Brskskss* 



C^OOL 



/ 




NAVAL POSTGRADUATE SCHOOL 

Monterey, California 




THESIS 



A SENSITIVITY ANALYSIS OF 
THE KALMAN FILTER AS APPLIED 
TO AN INERTIAL NAVIGATION SYSTEM 



Gary Glen Potter 
June, 1982 



Thesis Advisor: 



D. J. Collins 



Approved for public release; distribution unlimited 

T205755 




UNCLASSIFIED 



3CCUM1TY CLASSiriCATlOM Or THIS PACK (Whom Om< Bntmrod) 



REPORT DOCUMENTATION PAGE 


read instructions 
BEFORE COMPLETING FORM 


1 1 MPORT NUMIKR 


2. GOVT ACCESSION NO. 


3- RECIPIENT'S CATALOG NUMBER 


I 4. TITLE (end Subtitle) 

I A Sensitivity Analysis of the Kalman Filter 
as Applied to an Inertial Navigation System 


5 Type OF REPORT A PEAIOO COVERED 

Master's Thesis 
June, 1982 


s. PEAFOAMING oag. aepoat NUMBER 


17. AUTMORi'aJ 

Gary Glen Potter 


i. CONTAACT OA grant NLiMBCRC*; 


[9. PEAFOAMING OAOANIZATION NAME ANO AOOACSS 

1 Naval Postgraduate School 
J Monterey, California 93940 


10. program element, project task 

AREA A WORK UNIT NUMBERS ’ 


|l 1. CONTAOLLING OFFICE NAME ANO AOOACSS 

Naval Postgraduate School 
1 Monterey, California 93940 


12. REPORT OATE 

June, 1982 


13. number of pages 

104 


1 14. MONlTOAlNG AGENCY NAME 4 AOOACSS^/ dllloront Item Controlling Otllcm) 


II. SECURITY CLASS, (ol ihlo rmoort, 

Unci assified 


!§•. DEC LA SSI Fl CATION^ DOWNGRADING 
SCHEOULE 


[16. Ol ST Al BU T|OM STATEMENT (ot thle Report) 

Approved for public release; distribution unlimited 


1 17. DIST AltuTlON STATEMENT (ol the oka t reel ontarad In Block 20, II different from Report) 


I IS. SUPPLEMENT AAY NOTES 


I If- KEY WO AOS (Continue on rarer re oldo II nacaaeory end Identity ky klock n urn bar) 

Inertial Navigation Systems 
Kalman Filter 
1 Sensitivity Analysis 
Covariance Analysis 


[20. ASST A ACT (Continue on taroraa aide II nacaaeory end Identity ky klock number) 

A tactical missile with mid-course requires the use of an Inertial 
Navigation System (INS). Steady-state Kalman Filters (SKF) used as 
estimators have been proposed for use in a Strapdown INS that is con- 
sidered to be cheaper and easier to implement than a gimbaled INS. 

This thesis further investigates the sensitivity of the SKF to in- 
I accuracies in the filter parameters such as the dimensional stability 



DD ,'j 



FOAM 
AN 73 



1473 COITION or I NOV «s IS OBSOLETE 

S/N 0 102-014- 660 I | 



UNCLASSIFIED 




0 



|#cu 



UNCLASSIFIED 



***r 



CLiiH»*c*T<OM o» ▼ »«* » 



n*i« 



20. ABSTRACT (Continued) 



derivatives. The analysis is expanded to explore the sensitivity of a 
system of higher dimension created by the augmentation of an additional 
state. The study has been performed by independently varying each of the 
filter parameters over a given range and noting the effect on the accu- 
racy of the filter. One of the benefits of this analysis of the rms 
estimate errors to variations in the stability derivatives is that it 
reveals which derivatives need to be accurately determined to ensure 
stable flight. 



Approved for public release; distribution unlimited 



A Sensitivity Analysis of 



the 
to an 


Kalman Filter as Applied 
Inertial Navigation System 




by 



Gary Glen Potter 

Lieutenant Commander, United States Navy 
B.S.E.E., Univeristy of Idaho, 1973 

Submitted in partial fulfillment of the 
requirements for the degree of 



MASTER OF 


SCIENCE IN ENGINEERING SCIENCE 




from the 



NAVAL POSTGRADUATE SCHOOL 
June 1982 



ABSTRACT 

A tactical missile with mid-course requires the use of an Inertial 
Navigation System (INS). Steady-state Kalman Filters (SKF) used as 
estimators have been proposed for use in a Strapdown INS that is con- 
sidered to be cheaper and easier to implement than a gimbaled INS. 

This thesis further investigates the sensitivity of the SKF to in- 
accuracies in the filter parameters such as the dimensional stability 
derivatives. The analysis is expanded to explore the sensitivity of a 
system of higher dimension created by the augmentation of an additional 
state. The study has been performed by independently varying each of the 
filter parameters over a given range and noting the effect on the accu- 
racy of the filter. One of the benefits of this analysis of the rms 
estimate errors to variations in the stability derivatives is that it 
reveals which derivatives need to be accurately determined to ensure 
stable flight. 
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I. INTRODUCTION 



A tactical missile normally requires midcourse guidance to ensure 
that its trajectory leads to a specific target. A typical midcourse 
guidance law pre-programmed strategy maintains constant altitude, heading 
and speed. Such guidance is primarily effected by an Intertial Navi- 
gation System (INS). In this work two steady-state Kalman Filters (SKF), 
used as estimators of the longitudinal and lateral motion, constitute 
what may be considered as part of a Strapdown INS onboard a missile that 
can be cheaper and easier to implement than a gimballed INS. The authors 
of [Ref. 1] discuss the basic differences between Strapdown and gimballed 
Inertial Navigation Systems. 

Sensors on the missile that the longitudinal and lateral estimators 
could use are described by Maybeck [Ref. 2] and include laser rate gyros, 
doppler velocimeters , magnetic compasses, and barometric altimeters. A 
radar seeker could provide a distance or range measurement or range rate. 
Distance or position measurement could be computed from a signal inserted 
into the missile's INS from the Global Positioning System (GPS) or 
similar satellite-based navigation system. 

This work was motivated by Bryson [Ref. 3], where he discusses a 
Strapdown INS using SKF as estimators applied to the model for the DC-8 
airplane. To avoid classification requirements and for convenience, the 
model used here is essentially the same as that of [Ref. 3] rather than 
that of a missile. 
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This thesis is a continuation of the work done by Matallana [Ref. 4]. 
It further investigates the sensitivity of the Kalman Filter to inaccu- 
racies in the filter parameters or varation between the filter model and 
the plant model for longitudinal motion estimation. The differences 
could be due to model inaccuracies or to normal variation caused by a 
changing flight environment. The sensitivity of rms estimate errors to 
inaccuracies or differences in the stability derivatives is the result of 
interest. 

The initial work conducted was to reproduce the results of [Ref. 3] 
and [Ref. 4] with the correct implementation of the dynamics in the 
filter parameters. Then the results of [Ref. 4] for the longitudinal 
motion estimator with incorrect implementation of the dynamics in the 
Kalman Filter were reproduced. 

After a distance measurement and associated system and measurement 
noise parameters were added to the model dynamics, the sensitivity anal- 
ysis was repeated for the longitudinal motion estimator. The analysis of 
the effect that this distance input had on the sensitivity of the rms 
errors to inaccuracies or differences in the stability derivatives of the 
Kalman Filter concluded the research for this thesis. 
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II. MODELS AND ESTMATION 



A. KALMAN FILTER 

Only a brief description of the Kalman Filter has been included to 
show the particular formulation used. A more complete development of 
general theory is done by Gelb in [Ref. 5]. 

1 . Linear Dynamic System 

Consider the linear time invariant system (plant and measurement 
models) given by equation (1) below, where x represents the states of the 
system; z is the measurement; F is the system matrix; T is the driving 
noise coefficient matrix; H is the measurement scaling matrix; and w and 
v are independent, zero-mean, white gaussian noise processes with covar- 



iance matrices Q and R respectively. 

S’ 

x = Fx + Tw ( 1 -a) 

z = Hx + v (1-b) 

Mathematically, Q and R are represented by equation (2) as: 

E(w(t)w T (x)) = Q(t)CT(t-i), E(w(t)) = 0 (2-a) 

E(v(t)v T (x)) = R(t)a(t-x), E(v(t)) = 0 (2-b) 

2. Continuous Kalman Filter 



A continuous time Kalman Filter is described by equation (3) 
where x is the state estimate and K is a matrix of constant filter gains. 

x = Fx + K(z-Hx) (3) 
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The implementation of the System Model and the Kalman Filter is shown in 
Figure 1. 



MATHEMATICAL MODEL 




1 



Figure 1. System Model and Kalman Filter 
The estimate error is defined by equation (4) as 

<\< A » 

x si x - x (4) 

and the differential equation for x is given by 

x = (F-KH)x - Tw + Kv (5) 

The differential equations for the states of a linear system driven by 
noise can be expressed as 
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The covariance of the estimate-error, symbolized as P, is defined by 
equation (7). It provides a statistical measure of the uncertainty in x. 



P = E(xx T ) 



(7) 



The diagonal elements of the covariance matrix are the root mean square 
errors of the state variables. Also, the trace of P is the mean square 
length of the vector x. The off diagonal terms of P indicate the degree 
of cross-correlation between the elements of x. The covariance matrix P 
is obtained by solving the linear Lyapunov equation given by 

P = (F-KH) P + P(F-KH) T + TQr T + KRK T (8) 

The eigenvalues of the filter are given by the roots of 

ISI - F + KHI = 0 (9) 



B. STATE AUGMENTATION AND SHAPING FILTERS 

When the system random disturbances are correlated in time, i.e. , 
colored noise, it is necessary to use their power spectral density data 
in order to develop a mathematical model that produces an output which 
duplicates the noise characteristics [Ref. 2]. Correlated random noises 
are taken to be state variables of a ficticious linear time invariant 
system (usually called a shaping filter) which is itself excited by white 
gaussian noise. Such a model is given by equation (10) below, where the 
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subscript f denotes filter, and n is a nonwhite (time-correlated) gaus- 
sian noise. The filter output is used to drive the system depicted by 
Figure 2. 



= F f x f + r f w (10-a) 
z n = H f x f (10-b) 

The dimension of the state vector (1) is increased by including the 
disturbances as well as a description of the system dynamics behavior in 
appropriate rows of an enlarged F matrix. This enlargement process is 
called state vector augmentation. 



v 




Figure 2. Shaping Filter Generating Driving Noise 



The augmented state equation is given by 
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The associated measurement equation is 



H 


0 


X 






. x f. 



( 12 ) 



C. SENSITIVITY TO PARAMETER VARIATION 

Observing the structure of the Kalman Filter illustrated in Figure 1, 
the filter contains an exact model of the system dynamics. 

The analysis of how the error covariance behaves when the gain matrix 
is computed using perturbed values of the F matrix, such as varying 
parameters due to different flight conditions, is well explained in 
[Ref. 5]. Figure 3 is a block diagram of the system model and Kalman 
Filter with the system dynamics perturbed. F* is the perturbed system 

dynamics, while K* is the associated gain matrix computed for the Kalman 
Filter. 



SYSTEM KALMAN FILTER 




Figure 3. System Model and Kalman Filter with 
Perturbed Dynamics 
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The equation for the estimate is given by 



x = F*x + K*(z-Hx) (13) 

The error in the estimate is given by 

x = (F* - K*H)x + AFx - Tw + K*v (14) 

where 

AF - F* - F (15) 

The differential equations for the states of linear system driven by 
white gauss i an noise now become 
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Letting x' be the augmented state vector, x 1 
The covariance matrix of x 1 is given by 



E(x‘x ,T ) = 




(16) 



(17) 



where one defines P - E(xx^), V - E(xx^), and U - E(xx^). P, the covar- 
iance of x, is the quantity of interest. The error sensitivity equations 
are: 

P = (F* - K*H) P + P(F* - K*H) T + AFV + V T AF + TQr T + K*RK* T (18-a) 
V = FV + V(F* - K*H) T + UAF T - TQr T ( 18-b) 
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and 



0 = FU + UF T + TQr T 



(18-c) 



with initial conditions P(0) = -V(0) = U(0) = E(x(0)x(0)^). When the 
actual system dynamics are reproduced in the filter, F = F* and AF = 0, 
and equation (18) reduces to the linear Lyapunov equation of equation 
( 8 ). 

D. MODAL COORDINATES TRANSFORMATION 

The system represented by equation (1) is not unique. Consider an 
alternate linear transformaton of the states described in references [3] 
and [6]. Let x = T, where 4 represents the transformation of the states 
and T is the transformation matrix with the columns formed by the eigen- 
vectors of the system matrix F (for a complex eigenvalue, the first 
column is the real part and the second is the imaginary part of the 
eigenvector). The similarity transformation of equation (1) is 

4 = A4 + Bw (19-a) 
z = C4 + v (19-b) 

where A = T _1 FT, B = T"V, and C = HT. 

A case of particular interest, the canonical form, results when the A 
matrix is diagonal (i.e., when the eigenvalues of the F matrix appear on 
the diagonal). This canonical form is more informative than the transfer 
function method, since observability and control ability of the system can 
be obtained by inspection. 
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E. SOLUTION OF THE SKF WITH A PRESCRIBED DEGREE OF STABILITY 

The constant gain Kalman Filter (SKF) used as an observor will 
diverge if undisturbed, neutrally stable (UNS) modes are in the system 
model. In references [3] and [7] the authors discussed the destabili- 
zation of the system model (1). The amount of destabilization can be 
varied until the suboptimal observor formed has a desired degree of 
stability. The method of [Ref. 3] destabilizes only the UNS modes in the 
system model and is called "modal destabilization" (MDS). In this tech- 
nique the gains of the filter are constrained so that 

Re(S.) > -a, i = 1,2, ,n (20) 

where Re(S.j ) indicates the "real" part of (S^), S-|,...,S are the eigen- 
values of the filter, i.e., the roots of equation (9), and a is a speci- 
fied positive number. 

The original system model is destabilized in accordance with equation 
(21), where F 1 is the destabilized matrix formed, E is the destabili- 
zation matrix (diagonal), and T is the modal transformation matrix 
(eigenvector matrix). The matrix F' is used to calculate the suboptimal 
gains of the filter. 

F' = F + TET -1 (21) 

This MDS approach prevents the divergence of the steady-state Kalman 
Filter in a system with UNS model while causing only a slight reduction 
in the estimation accuracy. 
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III. DYNAMIC AND MEASUREMENT SYSTEM MODELS 



A. REFERENCE AXIS SYSTEM 

The Reference Axis System of a missile is centered at its center of 
gravity (c.g.) and fixed on the missile body as follows: 

X axis, the roll axis, forward from the c.g. along the axis of 
symmetry. 

Y axis, the pitch axis, outward to the right from the c.g. when 
viewing the missile from behind. 

Z axis, the yaw axis, downward from the c.g. in the plane of sym- 
metry to form a right-handed orthogonal system with the 
other two. 

Appendix A lists the symbols defining quantities associated with the 
missile illustrated in Figure 4 below such as forces and moments, linear 
and angular velocities, and moments of inertia. 




Figure 4. Reference Axis System 
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B. MISSILE EQUATIONS OF MOTION 



The equations of motion used to represent the missile dynamics used 
in this study are well defined in [Ref. 8]. A linear dynamical model of 
the missile based on the rigid body approximation is appropriate. 

1 . Longitudinal Motion 

The longitudinal motions of a missile can be modeled by a fifth- 
order system of equation (22), where the state variables are u, velocity 
along the X axis, w, velocity along the Z axis, q, pitch rate, 9, pitch 
angle and h, altitude. The units are: u and w in 10 ft/s, q in 0.01 
rad/s, 9 in 0.01 rad, and h in 100 ft. 
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2. Lateral Motion 

The lateral motions of a missile are modeled by the fifth-order 
system given by equation (23), where the state variables are: p, sideslip 
angle, r, yaw rate, p, roll rate, <}>, roll angle, and t|>, heading angle. 
The units are: p in rad, r in rad/s, p in rad/s, 9 in rad, and Qi in rad. 
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C. MODEL DYNAMICS 



The aerodynamic data used in this paper appears in Appendix B. 
Except for the addition of system and measurement noise parameters for 
the distance input, the models and noise dynamics are the same as those 
of [Ref. 3], 

1 . Longitudinal Motion Estimation 

The main disturbance inputs are the two wind velocities u„ and w . 

g g 

Under certain flight conditions, the turbulance represented by the fluc- 
tuating parts of Ug and w^ are colored noise. They are modeled by first- 
order shaping filters with white gaussian noise inputs as shown in 
equation (10). The linear model that results is given by equation (24) 
[Ref. 4]. 
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(24) 
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The numerical data for the longitudinal dimensional derivatives 
was used in equation (22). The resultant model is represented by equa- 
tion (25) which corresponds to the state vector augmentation of equation 



(11). Scaling is done with u, w, u , and w in units of 10 ft/s, q in 

y y 

units of 0.01 rad/s, 9 in units of 0.01 rad, and h in units of 100 ft. 



• 

u 




-0.015 


0.004 


w 




-0.074 


-0.806 


• 

q 




-0.749 


-10.7 


9 


= 


0 


0 


• 

h 




0 


-0.1 


u g 




0 


0 


w 

L 9 




0 


0 




“o 


0 




0 


0 




0 


0 




+ 


0 


0 




0 


0 




0.413 


0 




0 


0.853 



0 -0.0322 0 

0.824 0 0 

-1.344 0 0 

1 0 0 

0 0.0824 0 

0 0 0 

0 0 0 



-0.015 0.004 




u 


-0.074 -0.806 




w 


-0.749 -10.7 




q 


0 0 




9 


0 0 




h 


-0.413 0 




U 9 


0 -0.853 




W q 






- — 



The measurement model shown by equation (26) assumes a rate gyro 

in order to measure z and a barometric altimeter to measure z, . 

q h 



(25) 
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2. Lateral Motion Estimation 

The main disturbance input is the lateral wind v. The turbulence 
represented by the fluctuating part of v is the colored noise, which is 
also modeled as a first-order shaping filter with white gaussian noise 
input as given by equation (10). The resulting shaping filter taken from 
[Ref. 3] is given by equation (27). 



p = -0. 853p g + 0. 853p (27) 

where B = v „/V. 

g g 

The numerical data for the lateral dimensional derivatives was 
applied in equation (23) to obtain equation (28), which corresponds to 
the state vector augmentation of equation (11). 
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The measurement model given by equation (29) below represents the case 
where the measurement z is taken with a roll-rate gyro and the measure- 
ment z obtained from a magnetic compass. 




(29) 
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IV. ANALYSIS 



A. SIMULATION 

The Sensitivity Covariance Program developed for application in the 
work of [Ref. 4] was used to solve the error sensitivity equations of 
equation (18). The program was originally developed to handle a set of 
105 linear differential equations for the longitudinal case and 78 for 
the lateral. The program was revised to accommodate 136 linear differ- 
ential equations in the longitudinal case and 101 in the lateral, to 
allow for an additional state augmentation (i.e. , the distance measure- 
ment to the longitudinal model). The outputs of these programs are the 
time matrices and rms estimate errors, the square roots of the diagonal 
elements of the P matrices. The OPTSYS program, the use of which is 
described by [Ref. 9] and amplified by [Ref. 10], was applied to calcu- 
late the Kalman Filter gains to be inserted into the Sensitivity Covar- 
iance Program to find the estimate errors for specific system parameter 
perturbations. The OPTSYS program was also used to destabilize the 
systems that contained UNS modes in an attempt to eliminate filter 
divergence. Copies of the OPTSYS and the Sensitivity Covariance programs 
follow under COMPUTER PROGRAMS, while a copy of [Ref. 10] appears in 
Appendix C. 

B. RESULTS 

As the problem is introduced, the results are presented in three 
parts: (1) to verify the findings of [Ref. 3] and [Ref. 4] with the 
correct implementation of the dynamics in the filter parameters, (2) to 
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reproduce the findings of [Ref. 4] for the longitudinal motion estimator 
with incorrect implementation of the dynamics in the Kalman Filter, and 
(3) to conduct a sensitivity analysis of the longitudinal motion esti- 
mator after adding a distance measurement and associated system and 
measurement noise parameters to the model dynamics. 

1 . Motion Estimation Analysis for Exact Dynamics 

The OPTSYS program was used with input data representing the 
actual system dynamics for both the longitudinal and lateral cases to 
obtain the following results which are the same as those of [Ref. 4] and 
essentially the same as those of [Ref. 3]. 
a. Longitudinal Case 



filter gain 


matrix K 


filter eigenvalues 
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0.060 


-0.310 + jO. 41 1 
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-0.161 


-0.429 


3.517 
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0.001 


-0.080 


-0.261 


-0.011 
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-1.288 
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rms estimate errors 

u = 2.090 ft/s 0 = 0.317 deg 

w = 5. 102 ft/s ii = 8.245 ft 

q = 0.416 deg/s u^ = 4.776 ft/s 

S = 5.701 ft/s 
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b. Lateral Case 



filter gain 


matrix K 


filter eigenvalues 


0.051 


-0.967 


-2.350 + J2.594 
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-0.624 + jO. 492 
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-0.004 


-0.00125 
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-0.005 


0.906 
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rms estimate errors 

v 0 = 3.329 ft/s 
P 

r = 0.244 deg/s 
p = 0.377 deg/s 
<j> = 0.222 deg 
ijj = 0. 214 deg 
V Q = 5.506 ft/s 

p g 

2. Longitudinal Motion Estimation Analysis 

The 0PTSYS program was used to compute a new K* matrix as each 
parameter of the F matrix was individually numerically varied. The 
Sensitivity Covariance Program was then executed utilizing each new K* 
and F* matrix pair to determine the rms errors for each individual per- 
turbation. 

The results are shown in Tables 1-8 and are identical to those of 
[Ref. 4]. The true values for the unperturbed system dynamics parameters 
are indicated in the tables by an asterisk. A discussion of the results 
follows: 
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X . The dimensional variation of the X force with forward speed u 
has a nominal value of -0.015. This quantity was varied in a 
range of ±20%. The behavior of the rms estimate errors can be 

seen in Table 1. The tabulation shows that the numerical 
variation of the X u derivative does not cause significant 
changes in the nominal values of the rms estimate errors of the 
states w, q, 0, u g , and w g . The states u and h appear to be 
slightly effected, but not enough to be of importance. 

X . The dimensional variation of the X force with downward speed w 
has a nominal value of 0.004. Again, a numerical variation in 
a range of ±20% was conducted. The behavior of the rms errors 
is demonstrated by Table 2. Comparing these values with the 
nominal ones reveals that changes in the X w derivative have 
essentially no effect on the states w, q, 0, u^, and w^, while 
the states u and h show changes too small to consider important. 

Z u> The dimensional variation of the Z force caused by a change in 

. the forward speed u has a nominal value of -0.074. The design 
value was altered in a range of ±20% with the results shown in 
Table 3. Evaluation of this data indicates that all the rms 
errors show some sensitivity except for that of q. The most 
significant changes occur in the u, 0, and h states. The large 
variation in u can be important in terms of the accuracy in 
radial position. 

Z . The dimensional variation of the Z force with downward speed w 

has a nominal value of -0.806. The results for this case with 

changes in Z over a range of ±20% follow in Table 4. They 
w 

show that all the rms estimate errors are quite sensitive and 
any variation of Z^ beyond ±2% can be considered critical and 
unacceptable. 
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TABLE 1. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN X u DERIVATIVE 



X 

u 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u g 

ft/s 


w g 

ft/s 


-0.018 


2.096 


5.102 


0.416 


0.317 


8.248 


4.776 


5.701 


-0.0165 


2.094 


5. 102 


0.416 


0.317 


8.246 


4.776 


5.701 


-0.01575 


2.091 


5.102 


0.416 


0.317 


8.240 


4.776 


5.701 


-0.015 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.01425 


2.088 


5.103 


0.416 


0.317 


8.260 


4.775 


5.701 


-0.0135 


2.089 


5.103 


0.416 


0.317 


8.280 


4.775 


5.701 


-0.012 


2.092 


5.103 


0.416 


0.317 


8.340 


4.775 


5.701 



TABLE 2. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 

WITH VARIATION IN X DERIVATIVE 

w 



*w 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u 

g 

ft/s 


w 

g 

ft/s 


0.0048 


2.070 


5.102 


0.416 


0.317 


8.319 


4.776 


5.701 


0.0044 


2.080 


5. 102 


0.416 


0.317 


8.282 


4.776 


5.701 


0.0042 


2.086 


5. 102 


0.416 


0.317 


8.257 


4.776 


5.701 


0.004 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


0.0038 


2. 100 


5.102 


0.416 


0.317 


8.223 


4.776 


5.701 


0.0036 


2.103 


5. 102 


0.416 


0.316 


8.215 


4.776 


5.701 


0.0032 


2.110 


5.101 


0.416 


0.316 


8. 180 


4.776 


5.701 



30 



TABLE 3. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN Z u DERIVATIVE 



Z u 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u g 

ft/s 


w 

g 

ft/s 


-0.0888 


1.885 


5. 106 


0.416 


0.322 


9.310 


4.776 


5.707 


-0.0814 


1.974 


5.104 


0.416 


0.319 


8.808 


4.775 


5.703 


-0.0777 


2.026 


5. 194 


0.416 


0.318 


8.579 


4.775 


5.702 


-0.740 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.0703 


2.122 


5.100 


0.416 


0.315 


7.800 


4.777 


5.700 


-0.0666 


2.270 


5.095 


0.416 


0.313 


7. 101 


4.777 


5.697 


-0.0592 


2.32 


5.094 


0.416 


0.311 


7.000 


4.778 


5.695 



TABLE 4. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 

WITH VARIATION IN Z DERIVATIVE 

w 



Z w 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


ii 

ft 


u g 

ft/s 


w g 

ft/s 


-0.9612 


30.08 


6. 172 


0.486 


0.440 


17.97 


4.778 


7.138 


-0.8866 


11.80 


5.810 


0.428 


0.415 


33.50 


4.785 


5.957 


-0.8463 


5.345 


5.259 


0.421 


0.400 


22.776 


4.779 


5.737 


-0.806 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.7657 


2.668 


5.032 


0.412 


0.219 


16.903 


4.772 


5.710 


-0.7256 


3. 188 


5.035 


0.407 


0.200 


21.026 


4.760 


5.746 


-0.665 


3.260 


5.065 


0.406 


0.185 


23.000 


4.767 


5.794 
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M . The dimensional variation of the M moment caused by a change in 
the forward speed u has a nominal value of -0.000786. From 
Table 5, one notes that the rms errors for the states q, u^, 
and Wg are not effected by a variation in M u of ±20%, but 
significant changes are seen when M u is varied more than ±10% 
in the errors of states u, w, 0, and ii. 

M , The dimensional variation of the M moment with speed w has a 

nominal value of -0.0111. The results of a numerical variation 

in a range of ±10% can be seen in Table 6. Since any alter- 
ation in the true value of has a strong effect on all the 
rms estimate errors, this derivative can be considered the most 
critical in the longitudinal motion estimation case. 

M^. The dimensional variation of the pitching moment with pitch 

rate q has a nominal value of -0.924. The results of Table 7 

on the rms estimate errors for a ±20% change in show that 
the sensitivity to variations in this parameter is minimal for 
all states. 

Mv The dimensional variation of the pitching moment with the rate 
of change of the downward speed w has a nominal value of 
-0.00051. Table 8 contains the rms errors data obtained by 

altering M* ±20% from its nominal value. All the errors show a 
degree of sensitivity and the variation of errors is signifi- 
cant when M* is changed by more than ±2%. 

w 

Since plots of the rms estimate errors versus changes in the 
particular dimensional derivatives for data identical to that of Tables 
1-8 appears in [Ref. 4] in Figures 35-40, they will not be repeated in 
this work. A summary of the relative sensitivity of the rms estimate 
errors to changes in the individual dimensional derivatives follows in 
Table 9. 
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TABLE 5. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN M y DERIVATIVE 



M u 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u g 

ft/s 


w g 

ft/s 


-0.000943 


2.234 


5.061 


0.416 


0.305 


6.230 


4.775 


5.689 


-0.000865 


2. 115 


5.089 


0.416 


0.310 


6.640 


4.775 


5.695 


-0.000825 


2.104 


5.094 


0.416 


0.314 


7.531 


4.776 


5.698 


-0.000786* 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.000747 


1.993 


5.105 


0.416 


0.318 


8.595 


4.777 


5.703 


-0.000707 


1.866 


5.108 


0.416 


0.319 


8.832 


4.779 


5.705 


-0.000629 


1.566 


5.111 


0.416 


0.322 


9.178 


4.783 


5.708 



TABLE 6. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 

WITH VARIATION IN M DERIVATIVE 

w 



M 

w 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u g 

ft/s 


w g 

ft/s 


-0.01165 


18.43 


8.350 


0.502 


0.455 


29.870 


4.778 


5.695 


-0.0113 


5.163 


5. 142 


0.433 


0.373 


17.790 


4.778 


5.694 


-0.0112 


3.110 


5.113 


0.418 


0.321 


10.427 


4.777 


5.699 


- 0.0111 * 


2.090 


51.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.0109 


5.206 


5.018 


0.419 


0.325 


16.650 


4.776 


5.700 


-0.01055 


13.652 


5.342 


0.447 


0.430 


12.204 


4.776 


5.918 


-0.00999 


19.13 


18.95 


0.475 


0.704 


68.98 


4.756 


6. 102 
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TABLE 7. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN M DERIVATIVE 

q 



M q 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u g 

ft/s 


w g 

ft/s 


-1.109 


2.091 


5.102 


0.416 


0.316 


8.230 


4.776 


5.701 


-1.016 


2.091 


5. 102 


0.416 


0.316 


8.234 


4.776 


5.701 


-0.970 


2.091 


5.102 


0.416 


0.317 


8.236 


4.776 


5.701 


-0.924 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.880 


2.090 


5. 102 


0.416 


0.316 


8.244 


4.776 


5.701 


-0.832 


2.089 


5. 102 


0.416 


0.316 


8.236 


4.776 


5.701 


-0.7392 


2.089 


5.102 


0.416 


0.316 


8.232 


4.776 


5.701 



TABLE 8. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 

WITH VARIATION IN M* DERIVATIVE 

w 



M- 

w 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u g 

ft/s 


w 

g 

ft/s 


-0.00061 


2.610 


5.053 


0.417 


0.273 


10.665 


4.775 


5.688 


-0.00056 


2.476 


5.089 


0.417 


0.295 


6.301 


4.776 


5.703 


-0.00053 


2.398 


5.091 


0.417 


0.304 


4.150 


4.776 


5.698 


-0.00051* 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0. 00049 


2.491 


5. 108 


0.416 


0.322 


9.757 


4.776 


5.702 


-0.00046 


2.976 


5.118 


0.415 


0.332 


12.067 


4.776 


5.703 


-0.00041 


3.570 


5.124 


0.415 


0.340 


13.536 


4.776 


5.703 
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TABLE 9 



RELATIVE SENSITIVITY OF THE RMS ESTIMATE ERRORS 
TO CHANGES IN DERIVATIVES 



DERIVATIVE 


NS 


RS 


VS 


X , 


X 






u 








X 


X 






w 








Z 




X 




u 








Z 






X 


w 








M 




X 




u 








M 






X 


w 








M 


X 






q 








M* 




X 




w 









NS = Not sensitive 

RS = Relatively sensitive 

VS = Very sensitive 
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3. Analysis of Longitudinal Motion Estimation After Augmentation 



Including the distance measurement to the system required state 
augmentation of the system and measurement models as demonstrated by 
equations (30) and (31) that follow: 



• « — 

u 




-0.015 


0.004 


0 


-0.0322 


0 


-0.015 


0.004 


0 




u 


w 




-0.074 


- 0.806 


0.824 


0 


0 


-0.074 


- 0.806 


0 




w 


• 

q 




-0.749 


-10.7 


-1.344 


0 


0 


-0.749 


-10.7 


0 




q 


e 


= 


0 


0 


1.0 


0 


0 


0 


0 


0 




e 


h 




0 


- 0.1 


0 


0.824 


0 


0 


0 


0 




h 


“g 




0 


0 


0 


0 


0 


-0.413 


0 


0 




u g 


*g 




0 


0 


0 


0 


0 


0 


-0.853 


0 




w 

g 


_d _ 




-I - 0 


0 


0 


0 


0 


0 


0 


0_ 




_d _ 



0 0 0 
0 0 0 
0 0 0 



0 

0 

0.413 

0 

0 



0 

0 

0 

0.853 

0 



0 
0 
0 
0 
1.0 



,M d - 



(30) 
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and 




1 0 0 0 0 0 
0 0 1 0 0 0 
0 0 0 0 0 1 



u 

w 




( 31 ) 



w 



g 



d 



In these equations the state variables are u, velocity along the X axis, 
w, velocity along the Z axis, q, pitch rate, 9, pitch angle, h, altitude 
and d, distance traveled along the X axis. The units are: u and w in 
10 ft/s, q in 0.01 rad/s, 0 in 0.01 rad, h in 100 ft, and d in 10 ft. 

The OPTSYS program, when executed with the data from equations 
(30) and (31) above and the standard deviation values from Appendix B, 
yielded the following: 



filter gain matrix K 



0.0592 


0.0570 


0.0014 


0.2646 


-0. 1611 


-0.0007 


3.5168 


0.0404 


0.0002 


0.0011 


-0.0810 


-0.0007 


0.0135 


0. 1353 


0.0001 


-0.0114 


0.0356 


-0.0002 


-1.2876 


0. 1283 


0.0005 


0.0469 


0.0561 


1.0014 
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filter eigenvalues 



-3. 103 ± 4.1103 
- 1.000 
-0.4146 
-0.3152 

-0.0485 ± 0.0548 
-0.0551 



rms estimate errors 



u 


= 2. 


090 


ft/s 


9 = 


0.316 


deg 


w 


= 5. 


102 


ft/s 


h = 


8.225 


ft 


q 


= 0. 


416 


deg/s 


u g " 


4.776 


ft/s 


w 


= 5. 


701 


ft/s 


d = 


38.730 


ft 
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The next step in the analysis was to perturb each directional 
derivative independently by specific amounts from its nominal value and 
to observe the effect on the rms estimate errors. This process was 
carried out for all eight directional derivatives and the response was 
the same for each case — even a slight perturbation of -0.1% of any 
directional derivative from its nominal value caused the rms estimate 
errors to increase without bound. This behavior indicated that any 
incorrect implementation of dynamics in the new system formed by the 
augmentation of the distance measurement would cause instability and the 
Kalman Filter to diverge. 

Several system parameters were individually modified and the 
analysis repeated in hopes of finding a stable system for which the 
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Kalman Filter converged. The coefficient for the distance term in the 
process noise distribution matrix was varied from 0.01-5.0, the power 
spectral density process noise entry for distance was changed in a range 
of 1.105-30.0, and the distance term for the power spectral density 
measurement noise adjusted over a range of 0.03-30.0. None of these 
trials led to a stable system. 

A modal analysis was also performed using the open loop eigen- 
values from the 0PTSYS output listing. The system of equations (30) and 
(31) when transformed into modal coordinates give equations (32) and (33) 
below: 



V 




"o 


0 


0 


0 


0 


0 


0 


0 




h 






0 


0 


0 


0 


0 


0 


0 


0 




^d 


^sl 




0 


0 


-1.076 


2.957 


0 


0 


0 


0 




^sl 


^s2 


- 


0 


0 


2.957 


-1.076 


0 


0 


0 


0 




^s2 


^pl 




0 


0 


0 


0 


-0.006 


0.024 


0 


0 




^pl 


^p2 




0 


0 


0 


0 


0.024 


-0.006 


0 


0 




^ P 2 


|u g 




0 


0 


0 


0 


0 


0 


-0.413 


0 




^ U g 


3 w g 




0 


0 


0 


0 


0 


0 


0 


-0.853 




3\ 



~0 


0.1 


o" 




-1.0 


0 


1.0 




-0.028 


-0.088 


0 




-o.no 


-3.153 


0 




^ u 


1.027 


-0.048 


0 




^w 












-0.210 


1.467 


0 






0.420 


0 


0 




0 


1.836 


0 





( 32 ) 
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z q 




0 


0 


1.0 


0 


- 0.0001 


0.0005 


0.0548 


0.4802 


z h 


= 


1.0 0 


0.0016 


0.0016 


- 0.0156 


- 0.0612 


0.0082 


- 0.0025 


_ z d _ 




0 


1.0 


- 0.0008 


- 0.0004 


1.0 


0 


- 0.0657 


- 0.0252 





1 0 0 




V 








g 


+ 


0 1 0 




v h 




0 0 1 




v d 

— 



^sl 

^s2 

4 pl 

4 P2 

4u g 

t*n 



( 33 ) 



Consistent with the discussion in [Ref. 3 ], inspection of equa- 
tion ( 32 ) revealed that the energy mode 4g and the distance mode 4^, were 
neutrally stable (i.e. , eigenvalues = 0). Inspection of equation ( 33 ) 
showed that 4g was unobservable with z^ and and that 4^ was unobserv- 
able with Zq and z^. Equation ( 32 ) also disclosed that 4^ was undis- 
turbed by Ug and d, and that 4<j was undisturbed by w^. Therefore, de- 
stabilization was conducted in an attempt to prevent filter divergence. 
Both total and modal destabilization described earlier in this work and 
in [Ref. 3 ] were performed in amounts of 0.040 and 1.0 using the 0PTSYS 
program. The filter gains computed for the destabilized system were then 
executed in the Sensitivity Covariance Program with each of the modified 
parameter combinations discussed earlier. Without exception, the rms 
estimation errors increased without bound when the least sensitive dimen- 
sional derivative X u was perturbed by as little as ±1%. 
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V. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

The conclusions reached were based on the results obtained and will 
therefore be presented in three parts. 

1 . Motion Estimation Analysis for Exact Dynamics 

The data in the results is in agreement with that of both 
[Ref. 3] and [Ref. 4]. It shows that both the Kalman Filters for initial 
longitudinal and lateral cases are stable when the true values for the 
system dynamics are implemented. 

2. Longitudinal Motion Estimation Analysis 

The results from Tables 1-8 and summarized in Table 9 are con- 
sistent with those of [Ref. 4]. The stability derivatives Z w and M w 
cause the strongest changes in the rms estimate errors when they are 

varied. Basically, Z and M must be quite accurately reproduced in the 

w w 

filter to prevent divergence. Changes in the stability derivatives Z u> 
M , and M* reflect intermediate variations in nearly all the rms estimate 

u w 

errors. For the model considered a tolerance of more than ±5% affects 

the accuracy in the radial position since large variations in u occur. A 

tollerance of perhaps ±20% can be accepted in the dimensional derivatives 

X. X , and M for this model since no important effect is noted in the 
u w q 

rms errors over that range. 

3. Analysis of Longitudinal Motion Estimation After Augmentation 
From the results presented earlier for the new system model fomed 

by the augmentation of a distance measurement and associated process and 
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measurement noise parameters, it is apparent that the corresponding 
Kalman Filter will diverge for even a slight variation in any of the 
dimensional derivatives from their nominal values. Even the Kalman 
Filter developed by system destabilization proved to be unstable with the 
parameters used. 

B. RECOMMENDATIONS 

Further analysis of the augmented system including the distance or 
position estimation is desirable. Perhaps a more in-depth study of the 
measurement parameter scaling would enable the development of a stable 
Kalman Filter for at least a destabilized system. 
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VI. SUMMARY 



The sensitivity analyses performed in this work have revealed the 
importance of accuracy in determining system dynamics utilized in formu- 
lating the model for the Kalman Filter. The relative sensitivity of the 
rms estimation errors to variance in each of the particular dimensional 
derivatives is shown in Table 9 for the Longitudinal Motion Estimator. 

The longitudinal system augmented with the distance measurement 
developed appears to be extremely sensitive to variations in all the 
dimensional derivatives. Further analysis of the model developed is 
suggested. 
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APPENDIX A 
LIST OF SYMBOLS 



Regular Symbols 
A 
B 
C 
D 
d 
E 
F 
f 
F' 

g 

H 

H 

h 

INS 

K 

L 

M 

MDS 

N 

n 

P 

P 

Q 

q 

R 

r 

S 



Defi nition 

Modal transformation of F matrix 

Modal transformation of r matrix 

Modal transformation of H matrix 

Dutch roll mode 

Distance traveled along the X axis 
Destabilization matrix 
System dynamics matrix 
Subscript for filter 
Destabilized matrix 
Subscript for wind speed 
Measurement matrix 
Heading Mode 
Altitude 

Inertial Navigation System 

Kalman Filter gain matrix 

Rolling moment (about X axis) 

Pitching moment (about Y axis) 

Modal destabilization 

Yawing moment (about Z axis) 

Non-white gaussian noise 

Covariance propagation of the estimate 
error matrix 

Perturbed roll rate 

Covariance matrix of w 

Perturbed pitch rate 

Covariance matrix of v 

Perturbed yaw rate 

Spiral mode 
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Definition 



Regular Symbols 
SKF 
T 

UNS 

u 

V 
v 
w 

w g 

x 

X 

A 

x 

“V 

X 

Y 
z 
Z 



Steady-State Kalman Filter 

Transformation matrix 

Undisturbed neutrally stable 

Perturbed forward speed (along X axis) 

Forward velocity 

Perturbed side velocity 

Driving white gaussian noise 

Perturbed downward velocity 

Reference axis 

State vector of the system 

State estimate vector 

Estimate error vector 

Reference axis 

Measurement vector 

Reference Axis 



Greek Symbols 

4 » 

0 

* 

P 

r 



a 

a 

i 

X 



Definition 

Heading angle 

Perturbed pitch attitude angle 
Perturbed bank (roll) angle 
Sideslip angle 
Driving noise matrix 
Eigenvalue constrain 
Standard deviation 
Transformed state vector 
Time 



45 



APPENDIX B 

AERODYNAMIC DATA AND PROBABILISTIC INFORMATION 
v = 820 ft/s 



Longitudinal Model 

a. Dimensional Derivatives 

Xu = 0.015 1/s 

Xw = 0.004 1/s 

Zu = -0.074 1/s 

Zw = -0.0806 1/s 

Mu = -0.0786 1/s-ft 

Mw = -0.0111 1/s-ft 

Mq = -0.924 1/s-rad 

Mw = -0.00051 1/ft 

b. Distrubance Noise Standard Deviation 
a = a w = 1.105 1/s (10 ft/s) 2 

ct x = 30.0 1/s (10 ft/s) 2 

(7 ft/s rms gust with a 930-ft correlation distance). 

c. Observation Noise Standard Deviation 
Gq =0.15 s (0.01 rad/s) 2 

a h = 0.05 s (100 ft) 2 
a d = 30.0 s (10 ft) 2 
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2. Lateral Models 



a. Dimensional Derivatives 

Y v = -0.0868 1/s 

N' = 2.14 1/s 

P 

Nj. = -0.228 1/s 

Np = -0.0204 1/s 

L' = -4.41 1/s 2 

P 

L 1 = 0.334 1/s 

r 

Lp = -1.181 1/s 

b. Disturbance Noise Standard Deviation 
ct = 1.63 x 10“ 4 1/s 

(7 ft/s rms gust with a 930-ft correlation distance) 

i 

c. Observation Noise Standard Deviation 
0 p = 1.5 x lO -5 s 

= 1.5 x 10" 5 1/s 
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APPENDIX C 

AN AID TO USING OPTSYS AT NPS 



I. INTRODUCTION 

One of the tasks involved in my thesis work at the Naval Postgraduate 
School (NPS) was to verify some of the data of reference [1] which in- 
vestigated the sensitivity of the Steady-State Kalman Filters as lateral 
and longitudinal estimators in Strapdown Inertial Navigation Systems 
(INS). One of the recurring, essential calculations was for the steady- 
state gains of each system model considered. Fortunately, the OPTSYS 
computer program was available in Fortran at the computer center to help 
perform this enormous job. The use of the OPTSYS program was covered by 
reference [2], but not in adequate detail for easy application. After 
much trial-and-error , frustration, attempted decoding with the assistance 
of the computer center staff, and prayer, and at the expense of many 
man-hours of time, our Lord enabled me to properly fill out and order the 
data cards for a particular modeled system and obtain the expected 
results upon execution of the program. Since Professor Collins has 
several other students in need of a users working knowledge of the OPTSYS 
program and anyone using Kalman Filters can benefit as well, I am writing 
a more detailed description of how to correctly input data by discussing 
a specific example. The intent of this paper is to supplement the 
guidance of reference [2] and further facilitate research at NPS. 
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II. MODEL AND ESTIMATION 



Consider the linear time-invariant system given by 

x = Fx + Tw 
z = Hx + v 

where x represents the states of the system; z is the measurement vector; 
F is the system matrix; T is the driving noise coefficient matrix; H is 
the measurement scaling matrix; and w and v are independent, zero-mean, 
white gaussian noise processes with covariance matrices Q and R, 
respectively. 

A continuous time Kalman Filter for this system is described by 

x = Fx + K(z - Hx) 

where x is the state estimate and K is the matrix of the steady-state 
gains of the Kalman Filter. The implementation of the System Model and 
the Kalman Filter are shown below in Figure C-l [Ref. 1]. 
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MATHEMATICAL MODEL 







Figure C-1. System Model and Kalman Filter 
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III. AN EXAMPLE OF LONGITUDINAL MOTION ESTIMATION 



After state vector augmentation, the resultant model of longitudinal 
motion of an aircraft of the form x = Fx + Tw is 



• 

u 




-0.015 


0.004 


w 




-0.074 


-0.806 


* 

q 




-0.749 


-10.7 


• 

e 


= 


0 


0 


• 

h 




0 


- 0. 1 


♦ 

u 

g 




0 


0 


“g 




0 


0 




“o 


0 




0 


0 




0 


0 




+ 


0 


0 




0 


0 




0.413 


0 




0 


0.853 



0 -0.0322 0 

0.824 0 0 

-1.344 0 0 

1 1 0 

0 0.0824 0 

0 0 0 

0 0 0 




-0.015 0.004 




u 


-0.074 -0.806 




w 


-0.749 -10.7 




q 


0 0 




0 


0 0 




h 


-0.413 0 




u g 


0 -0.853 




w q 






y 



where the units are scaled such that u, w, u , and w must be multiplied 

y y 

by 10 to give feet per second, q by 0.01 to give radians per second, 9 by 
0.01 to give radians, and h by 100 to give units of feet [Ref. 1]. 
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The corresponding measurement model in the form z = Hx + Iv is given 



by 



u 

w 



z ; 




0 0 1 0 0 0 0 




q 




"l 0 




V 


q 


- 






e 


+ 






q 


id 




0 0 0 0 1 0 0 




h 




0 1 




_ v h 




(26) 



For this model Q u = Q w = 1-105 (10 ft/s) 2 /s, R^ = 0.15 (0.01 rad/s) 2 and 
R h = 0.05 (100 ft) 2 s [Ref. 3]. 
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IV. APPLYING OPTSYS TO THE EXAMPLE 



The essential input data that will enable OPTSYS to calculate the 
steady-state gains of the Kalman Filter and many other parameters out- 
lined in [Ref. 2] follows on page 55. The input data and control cards 
are described in the paragraphs below. 

Card 1 - The 17 entries in every other column from column 2 through 
column 34 essentially tell OPTSYS what to compute. See [Ref. 2] for more 
details. 

Card 2 - The 5 entries in every third column from 3 through 15 de- 
scribe the system being modeled to OPTSYS. The first entry tells the 
number of states or order of the system-7 since there are seven rows in 
the F matrix. The second entry gives the number of controls-0 since u=0. 
The third entry tells that we have 2 measurements, while the fourth entry 
shows that two process noise sources exist. The fifth entry is always 
zero when filter synthesis is done. See [Ref. 2] if regulator synthesis 
only is desired. 

Cards 3-16 - These cards contain the F matrix. The first six entries 
of each row go on one card with 12 columns for each entry- 1-1 2, 13-24, 
60-72. The seventh entry for each row is placed in columns 1-12 of 
a continuation card that immediately follows the card with the first six 
entries of the row. Note that if our example system were 6x6, the F 
matrix would only take up cards 3-8. 
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The next three cards, 17-19 in our example, contain the H matrix. 
Note that this matrix is also entered on the cards by rows, but consecu- 
tively with an entry in every 12 columns with 6 entries per card as long 
as unused row elements remain! Thus the first entry of row 2 of the H 
matrix appears in columns 13-24 of card 18. 

The next three cards, 20-22, hold the F matrix. This matrix is also 
entered consecutively by rows with an entry in the first 14 groups of 12 
columns on the cards! 

The next to the last card gives the Q matrix. Note that this card 
has only the diagonal terms of the matrix in columns 1-12 and 13-24. See 
[Ref. 2] for matrices with non-diagonal terms. 

The last card is for the R matrix and also has diagonal entries in 
columns 1-12 and 13-24. Again refer to [Ref. 2] if non-diagonal terms 
exist. 

This supplement will be effective until the OPTSYS program is 
re-coded in WATFIV language. Its usage should greatly improve the 
efficienty and morale of those using the OPTSYS program on file at NPS 
Computer Center. 
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COMPUTER OUTPUTS 



C/OPTSYS1 JOB (1480,0417) , 1 POTTER G.G. 
C/ EXEC PCRTCLG 
C/SYS IN DC * 

IMPLICIT REAL&8 (A-H.O-Z) 
DIMENSION ACL (16,16), B (B t 9) 

^CWI ( 16 ) * ~ * * 



CFRO (16 
*V21 ( 1 6 



ION ACL (1 6 , 16) , B (B t 9| ,3 A ( 16,16) , Cl f 1 6) . CR ( 1 6 ) t CQ ( 1 6 , 1 6) , 

} . CWP. ( lb) , f BG 0(8.1 6) , FBGE (16,6) ,3 ( 1 6 , 16) , ill ( 1 o . 1 6) , 

,16) , RC (3 , 9) , SC (16,16) , Wfi ?3 2) ,*1(32) . K11(16-16f, 

, 16) , Xf 32 ,32) -GNM6, 1 6] ,H0 (8, \S) . D1 f 5 2) ,D2 (32) . RM (32,32) , 
-GAM (16,3) , WNOR Mil 6.1 6) , WNORMI ( 16 , 16) .DESTAB (16) , 

^16) , 3M n6,8) ,CK (8. 16) -fc (8, 8) ,DST03E (16. 16) , 

£0C? f3 2l.P.|s (|2r - AY (8) , BB (3 2 ) , CC (32) , CP ( 1 6) ,g6(3^,8) , GV(32,8) , 

°H Y ( 8 , 32 ) ,r:U (8,32) 

EQUIV AL^NCE (WI 1 (1, 1) ,GW (1 . 



(9 . 8 ) - « 
CAA (U .S 6 
:F ]3 5 



r 0 V (1,1) ) , 



equivalence (Wi i (1, i) ,gw n . i) ) , (wi i (i , i) ,c 
1 (’-21(1,1) rHYM-in, (U21M , if, HU (1,1)) 

COMMON /FROG/ I OL , I NC , IQ , IR , ISS , I M , I I* F 1 , ITF 2 , ITF 3 , IF DF W , I E , 

* IDS?AE,IDZBU3,ISST, ISEG, I PSD, I YU, INORM 
DATA ST A n/ ' ^ 1 / 

4 FOR M.AT(40l2) 

10 1 READ (5, 4 - ZND=100) IOL , IQ, IN Q, IB ,ISS, IM, ITF1 , ITF2 , I TF 3, IFDFW, I E, 

0 IDSTAB- IDEBUG,ISET,IPSD -I YU, I NORM 
READ (5,7435) NS, NC, NOB,NG, I ft EG 

743 2 FOR M AT (513 ) 

WRITE (6 -4000) NS.NC.NOB, N3 

4000 FOR H A * ( ' 1* ,2x,* ORDER OF SYSTEM =• 13, //, 2X, 1 NUMBER OF CONTROLS =' 

1 , 13.//, 2X* NUMBER OF OBSERVATIONS =',I3,//,2X, 

2 • number of process noise sources =',I3,//) 

N2=23NS 

CALL INKER (NS.NC.NOB, NG- N2 , ACL, B,3A,CI,CR,CQ.CWI , CWR , D, FBGC, F3GE, 
OG,G AM,GM,GN.H0- Dl , D2- FEO.RM , RC. Q , SC , W R - W I - W 1 1 , W2 1 , X , 

*WN0RK, W NOR MI, DESTAB, A A,BM, C M , JC t , RES , A Y , BS , CC , CP , GW , G V , HY , HU , 

* PS TORE) 

99 READ (5,5. END* 100) STA 



5 FOR MAT ( a2) 

IF(ST AR. r 
GOTO 99 
100 STD F 
END 

SUBFOUTINE SETUP (BA ,G ,GA M, N S, NC , NG) 

RETURN 

END 

SUBFOUTINE 



. EQ.STA) GOTO 10 1 



+ INNER (NS, NC, NO-NG. N2, ACL,B , BA, Cl - CR-CQ, CWI-CWR, D, FBGC, FBGE, 

*>G,J AM,GM-GN,HO,D1 . D2. TRO r & M , RC. 6, SC ,rf 6 . W I f W 1 1 , W2 1 - X, 

^ V ND Ft M , W N uR M I , Di ST A E , A A , 6 M , C M , JC r , P ES , A Y , H 6 , CC , CF , G W , G V , HT , HU , 

C* DSIORF) 

IMPLICIT REAL r, 8 (A-H,0-Zl 

F N SI ON ACL(Nb-MS) , BlNC, NC) -B A (NS , NS) , Cl (NS) , C R (NS) ,C 
( NS) , CW R (NS) . r BGC ( NC , NS ) , FBGE (NS - NO) ,G(NS,NSl , CM (NS, 




IsfSi,”'” 1 ' 



R.1( 

(NO 

h) 



DSt6r p jss^ fjS) 

io.MrON /?r6c/ IOL, I NO, I g. I R , ISS, IS. ITP1 - ITF2 , ITF 3 , I F D F U , I E , 
I DSTAB, I DEBUG,! Sc *,I RE3 ,IPS&, I YU, I NORM 



FEAL*4 p MT (20) 

NSQ-NS^NS 
C 

a 5 * O UTPU T OPTIONS 

C I CL= 1 IF THE OPEN LOOP EIGENS YSTEM IS DESIRED-- OTHERWISE IOL = 0 

C 10=1 IF THE RMS VALUES OF THE CONTROL AND STATE ARE TO BS FOUND 

C 1 NO = 1 IF ONLY B AND P ARE DIAGONAL 

C I Nv-0 IF A, B, Q- AND R APE DIAGONAL 

C 1 F = 0 IF OPTIMAL FILTER AND REGULATOR El GEN ST STEMS ARE TO BE FOUND 

C I P = 1 IP EXTERNAL C MATRIX IS SUFPLIZD 

C I R=2 IF EXTERNAL K IS SUPPLIED 

C I F= 3 IF EXTERNAL C AND K APE SUPPLIED 

C I SS = 1 IF STEADY STATE V ALU ES APE TO 3E DETERMINED 

C X M= 1 IF MODAL STATES DESIPEO 

C 
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onnn n o non 



3 FOR F AT ( * ' 'OPEN LOOP DYNAMICS MATRIX....',//) 

74 4 4 FOR MAT(6E12.5) 

60 0 0 FORMAT? 10 (2X, 1PD10.3) ,/ t 2X. 10 (2X.1PD10. 3) ) 

6001 FOR M A T ( * O' # //, 2 X , * T HE CONTROL DISTRIBUTION MATRIX ' , //) 

6003 FORKATi'O' ,//,2X,*THE CONTROL WEIGHTING MATRIX • , // 

6010 FOR MAT (' O' , // ,21 , ’PRO CESS NOISE DISTRIBUTION MATRIX • , //) 

6011 FORMAT/’ O' ,//,2X, •POWER SPECTRAL DENSITY - PROCESS NOISE • , //) 

605 1 FORMAT?* O' ,//.2X •MEASUREMENT SCALING MATRIX ',//) 

6052 FORMAT?* 0* ,/,2X.‘ POWER SPECTRAL DENSITY - MEASUREMENT NOISE..* // 

6012 FORMAT?* 0* ,//,2X, * DIAGONAL OUTPUT COST MATRIX...',//) 

6013 FCRMAT('0' ,//,2X, 'OUTPUT COST MATRIX * , //) 

8515 FORMAT?* O' ,//,2X, 'MEASUREMENT FEEDTHROUGH MATRIX...',//) 

MH = NS 
tt = N2 



SUEROUTINE CHECK CHECKS THE CONSISTENCY DP REQUESTED OPTIONS 

CALL CHECK (EPS, NC, NG. NO) 

IF ( I S ET- EQ . 1) GO TO SO 15 
DO 9010 1=1, NS 

9010 READ(5,7444 ) (B A (I , J) , J= 1 . N S) 

I F ( I D ST A B .EQ. 0) GO TO 9^14 
REA D ( 5.744 4) (DESTAB(I) ,1=1 ,NS) 

9014 CONTINUE 
GO TO 9016 

9015 CALL SETUP (3A,G, GAM, NS, NG, NC) 

9016 CONTINUE 
WRITE J6, 3) 

DO 5001 1= 1,NS 

50 0 1 WRITE (6 .60 00[ ( BA (I . J ) . J = 1 , NS) 

IFfIDSTAB .EQ. 0) GO TO 3999 
WRITE (o , 480) 

480 FOR M A T (/, ' bs ST A BIL IZ ATION CASE....'./. 

* * THE FOLLOWING VALUES WILL BE ADDEt) DOWN THE DIAGONAL TO ', 

* 'DESTABILIZE THE'./.' ABOVE MATRIX. OPTIMAL GAINS FOR THE ', 

* 'DESTABILIZED SYSTEM ARE THEN USED' /, 

' AS FIXED SUBOPTT MAL GAINS WITH THE .ABOVE SYSTEM',/) 

WRITE (6.6000) ( DESTAB (I) ,1= 1 , NS) 

3^99 CONTINUE 

if EIGENSYSTEM OF THE OPEN LOOP DYNAMICS 
IF (IOL. FQ. 0. AND. IQ.EQ.O) GO TO 500 

IF ?IOL. EQ. 0. AND. NC.NE.O) GO TO 500 

DO 51 I = 1 ,NS 

DO 51 J = 1.NS 

51 GN(I.J) = BA(I.J) 

' n»r»CS!»5rf > #'Ai t-.r-nunt. fc|r f 

CALL SALANC (NS, N S , G N . LOW.IH IGH, D1) 

CALL ORTHFS ?NS , NS , LOW , I H I G H , G N, D2) 

CALI OPTRA N ?NS. NS. LOW , IH I G H . GN. D2 . SC) 

CALI HQR2 (NS, NS , LOW .1 HIGH, G N.CWP..CWI. SC, I ERR) 

IF ( I E RH .NE. 0) CALL ERE XI T (NS, GN , IER R) 

CALL EALPAK (NS. NS, LOW ,IHIGH.D1.NS,SC) 

* f - r •fiT'Sf-r +r'ftu*in}* rtb 9 & i a 



NORMALIZE AND PRINT OPEN LOOP E IGEN S YSTEM 
IWRITE * 1 

CALL CNOFM ( CW R , C W I , SC , NS , I W RITE , NSQ , D DD , D 1 , D2 , WN C R M , W NOPM I , HO , CM , 
* NO, NS) 

IF( IOL. EQ. 2) RETURN 

IF (IQ. EQ. 0. OR. (NC.NE.O. OR. IDSTAB.GT. 0) ) GO 70 500 
DO 496 1=1 , NS 

IF (CWR (I) . LT.O.) GO TO 49S 
WRITE (6,u95) 

495 FORMAT (///’ PROGRAM TERMINATING DUE TO UNSTABLE SYSTEM') 

RETURN 

496 CONTINUE 

IF (IOL .EQ. 3) GO TO 510 
DO 497 1*1, MS 
DO 497 J=1 , NS 
^97 VII (I J) *SC (I .J) 

CALL MINV (N3Q, W1 1 , WS , DDD, D 1 , D2) 
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500 



C 



505 



506 



507 

510 



1806 

8520 

8504 



8517 

8519 



95 10 

9210 

8500 

9120 

9020 



9505 
q 2 1 5 



9216 
92 17 



9218 

9219 



9508 
90 3 0 
90 35 

9520 



904 0 



CONTINUE 

IF (ID STAB . EQ. 0) GO TO 510 

PORS {j ^ D I AG (D £ STAB) * 0-IN7 

DO 505 J=1 , NS 
DO 505 1=1 , NS 

A A ( J . J) = WNORH (I, J) * DESTAB (J) 

DO 507 1=1, NS 
DO 507 J=1 , NS 
ODD = 0. DO 
DO 506 K=1, NS 

DDD = ODD ♦ A A (I, K) ^VNOP.MI (K,J) 

PST OR E ( T , J ) = DD D 

S A ( I , J) = 5 A (I, J) ♦ DDD 

CONTINUE 



IF (NC . E Q. 
IF(IOL .EC 
IF (IS. NE. 



Q. 0) GO T 
Q. 3} GO T 
1 . AND. IS. N 
IFjlSET. EQ. 1) GO TO 95 It 



READ (5,744 4) ( ( HO (I , J ) , J = 1 , NS) , I = 1 , NO) 

WRITE ft>, 6051) 

DO 1806 I = 1 , N 0 

WRITE (6,60 0 0) (HO (I , J ) ,J = 1 , NS) 

IF(IM.NE.I) GO TO 9 50 4 

CALL MODE ( W NO Rfl , HO , CH , NS , NO , NS, 2) 

CONTINUE 

IF ( I F DF W .EQ. 0) GO TO 8519 

READ{ 5,7444) ( ( b ( I , J) , J= 1 , N C) , I = 1 , NO) 

WRITE (b , 85 1 5) 

DO 8517 T=1.NO 

WRITE (6,60 0 0) (D (I , J) , J= 1, NC) 

CONTINUE 
NOB =• 0 

TO 1301 
TO 9035 

. N E. 3) GOTO 9120 
95 10 

,J = 1 , NC) ,1 = 1, NS) 

READ (5, 7 444) ( ( FBGC (I ,J) ,J = 1 , NS) ,1=1, NC) 

WRITE fb .600 1) 

DO S 2 1 6 1 = 1. NS 

WRITE (6,60 00) ( G (I . J) . J= 1 , N C) 

IF(Ttt.NE.l) GO TO §500 

CALL HOD E ( V NO RH I , G , BH , NS , NS , NC, 0 ) 

CONTINUE 

GOTO 6399 

DO 90 20 1=1 , N S 

DO 9020 J=1.NS 

P H ( I ♦ flH , J) =0. 0 

IF? INQ . EQ. 1) GO TO 9215 

DO q 5 05 J=1,NS 
A A ( J , J) = HO (I,J)ftAT (I) 

GO TO 9 2 1 Q 

P EA D ( 5,744 4 ) { ( Q (I , J) ,J=1,NO) ,1=1, NO) 

DO 9217 1=1, NO 
DO 9217 J = 1 , N S 
DDD = 0. DO 

DO 9216 K = 1 , N 0 

DDD = DDD ♦ Q (I , K ) * HO ( K, J ) 

AA(l.J) = DDD 
WRITE (6,6013) 

DO 9218 1=1. NO 

WRITE fb, 6000) (Q(I,J) , J= 1 , NO) 

DO 9509 1=1 , NS 
DO q 5 09 J= 1 , N S 
DO 9508 F=1.NO 

RF. (I*HH,J) = nF(I*HH,J) ♦ A A ( K , I) ~ HO ( K , J ) 
IF) ISET. EQ. 1) do TO 95 20 

?EAD(S,74U4) ((G(I,J).J = 1 ,NC) ,1 = 1, NS) 

IPflOL . EQ. 3) GO TO §045 

CONTI NUE 

DO Q 0 4 0 1= 1 , NC 

DO 9040 J= 1 ,NC 

3 (I , J) =0.0 
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non 



R2AD (5,7444) (B(I,I),I=1,NC) 

IFfINQ -EQ. 1) GO TO 9045 



WRI TE (6,60 12) 

WRITE (6,60 0 0 (AY (I) ,1=1 , NO) 
WRITE (6, 6001 
DO 606 I = 1 , NS 
606 WRITE (6, 6000) (G(I c J) t J = 1 , NQ 
IF(IM.NE.I) GO TO 6S01 
CALL MODE (WNOSrtI, G, B M , NS , NS , NC, 



0) 



8501 CONTINUE 
IP ( IOL . EQ. 3) GO TO 8505 
WRf TE (6, 6003) 

DO 607 1 = 1 . NC 

607 WRITE f6, 6000) ( B (I , J) , J= 1 . N C) 

8399 I F ( IT r 1 .EQ. 0) GO TO 840& 

OPEN LOOP TRANSFER FUNCTIONS 

8505 WRI TE (6. 9220) 

9220 FORMAT(' O' ,//,2X, 'OPEN LOOP TRANSFER FU NOTIONS. . . * ) 

I T F X — 1 

CALL TF (NS ,NS,NSQ, BA, A A, NT, G,BM, NO , HD , C!1 , 1 FDFV, D , SB, CC , CP , 

* w R, WI, CWR ,CWI ,SC, JCF.RES, D1 , D2,DDD, EPS, ITP1, ITFX) 

8400 IF( IOL . Nfe. 3) GO TO 850i 
IF(NG . EQ. 0) RETURN 

GO TO 625 

8502 CONTINUE 

I F ( I R . EQ. 1 -OS. IR -EQ. 3) GO TO 9130 

+ COP C OC'CS v 

C***CALCU LATION OF CONTROL GAI NS : FORK AT ION OF CONTROL HAMILTONIAN 
C 

C or AND FT APE THE OPEN LOOP 

C « * DYNAMICS MATRIX AND TRANSPOSE 

C ** F -GflfrBI^GMT* Ofr*BI IS NCXNC CONTROL WEIGHTING 

CO * MATRIX 

C * * *** A IS THE NSXNS STATE WEIGHTIN 

CO > MATRIX 

CO -a -FT * 

C o* ** ***GM IS THE NSXNC CONTROL 

C DISTRIBUTION MATRIX 

QC. > tf-Afr-W r*Orti r*** O* t'lOC 

DO 24 I = 1,NC 
DO 24 J = 1 , M H 

24 FROfI,J) = G (J,I)/B(I,I) 

DO 25 I = 1,MH 

DO 25 J = 1 -MH 
Rfl(I. J*MH) =0. 00 
DO 25 K = 1,NC 

25 RM(I,J*MH) = P.M(I f J*KH) - G (T f K) ft PRO ( K, J) 

C* ** 2 NX2N H AMI LTONI AS MATRIX**'****^^^***** 



c-- 



• D I AGO N A L BLOCKS Mil AND K22 

DO 26 I * 1 , M H 



DO 26 J = 1.MH 
R M ( I , J) = PA (I,J) 
PMJI* MH, J* MH) = -BA (J , 



I) 



C M 2 1 3 LOCK 

26 Frtri + MH,J) = -RM(T*MH,J) 

C M12 BLOCK IS DEFINED IN LINE 25 ABOVE 

Qt- far- 

50 CONTINUE 

I P { I D EBUG .EQ. 0) GO TO 1050 
WRITE (6,6014) 

6014 FOP MAT (//, 1 EULEP-LAGRANGE SYSTEM MATRIX...',//) 

" ‘ - - ^ - ipd 1 3. 6) ) ' ) 



CALL FAP&&7 (M ,M ,M, 9. RK,4 . • ( 9 ( IX, 1PL 
1050 CALL BALANC(M,M, rM. LO W, I HI 2 H , D1 ) 

CALL ORTHES (M , M , LOW , I HIGH, 2 M , D2 ( 

CALL OPTRA N (M , ■ .LOW. I HIGH. R M, D2 , X) 

CALL HQR2 (R,M f L0« f I HIGH, RM, WP,W I, X, I ERF) 
IF( IERR .NE. 5) CALL EP 5 XI i f M , R M , I S ? R ) 
CALL 5ALFAF. (M , - . LOW I HIGH, D 1 . M. X ) 

C^. kforopf-f putir6tir|4M»f 

C DEBUG DIAGNOSTICS ON E-L EQ 



60 



non noon non 



IFfIDEBOG .EQ. 0) GO TO 53 
WP.I TE (o,91 15) 

9115 FOR HAT]//' ZIGVAL AND EIGNVEC OF 2^N E- L EQ. AFTER HQR2 ' //) 
DO 52 I = 1.M 

52 WRI?SJ6,91 16) WR(I),KI(I) 

9116 FOR “AT (1L 1P2D13.6) 

WRITE (6.91 17) 

9117 FOR MAT ( * 0 ' ) 

CALL P.APRS T (M,M ,M, 9,X,4, • (9 (1 X, 1 PD1 3- 6) ) • ) 

5 3 CONTINUE 



IF ( IDSTA E .EQ. 1) GO TO 54 

— WRtTI 

__ , , WRITE.. 

54 IF (NOB. N E . 0) GO TO oO 



IF (NCB.EQ.O 
IF (NOE “ ‘ 



. EQ. 0) WRITE (6 ,91 19) 
-NE.ol WRITE j6 c 91 2l( 
- N E • 0) n 



9119 FORHATC O' ,//,2X, • EIGENSYST EM OF OPTICAL CLOSED LOOP SYSTEM..' // 
9121 FORMAT?' 0* , //.2 X , 1 EIG ENS Y5 T EM OF ESTIMATE ERROR EQUATION ' , // 

Call rgai N(m. ns,nc.nob,wr,vi,x,gn,wi i , rm, 

1W21 ,D1 ,C WR,CWI, SC, MHS ,D2) 

C CHECK EIGVEC 

IF f I DEBUG .EQ. 0) GO TO 750 
WRITE (6.9125) 

9125 FOR M A T ( ' EIGENVECTORS FROM RGAI N PRIOR TO CNORM') 

CALL FA?RNT(NS,NS,NS,9,SC,4 , ' (9(1X,1PD13.6)) ') 

750 CONTINUE 

RESET FLAG AND F MATRIX FOR ITERATIVE DESTABILIZATION CASE 

IF (IDSTAB .EQ. 0) GO TO 9136 
DO 9135 1=1. NS 

9135 3 A (1,1) = BA ( I , I ) - DESTAB(I) 

IR* 1 

9136 CONTINUE 

CALCULATION OF FEEDBACK GAIN 



T * $ F EFDB ACK GAINS 0 

CALCULATE GT 

DO 801 I = 1, SC 

DO 801 J = 1 , NS 
PFO (I -J) = 0. DO 

DO 800 k = 1, MH 



• (BINVER SE) * GT 5 GN 



ukj rv — ■ , i.n 

900 PRO (I , J) = ?ftojl,J) *G (K,I) ^GN(K, J) 
801 FBGC.fl.J) = - PR 0 ( I , J) /3 (1,1) 

IF (IDSTAB -EQ. 1) GO TO 9130 



NORMALIZE AND PRINT OPT. REG. C LOSED LOOP EIGENSYSTEM 
I WR IT E =2 

CALL CNORM (C W R , CWT , SC , NS , l W PI TE , N SQ , DD D, D 1 , D 2 , WN 0 R M, W NOP MI , FH GC, 
* A A, NC, NS) 

C^ 5 *^ THE OPTIMUM FEEDBACK CONTROL GAINS 
9130 WRITE ( 6 , 977 ) 

977 FOR MAT (/// , ' '.'THE CONTROL GAINS ARE:',//) 

DO 96 8 I = 1,NC 

96 8 WRITE ( 6.9781 (F 3 GC]I,J),J * 1 .NS) 

978 FORMAT]* ' ZX . 1 P 6 0 T 4 . 6 , / , 2 X , 6 D 1 4 . 6 ) 

C COMFUTS MODAL C MATRIX 

COPEN LOOP D-INVE SAVED IN WNORMI 
I F { I M - NZ. 1 ) GO TO 985 
C IN COMPUTING MODAL C RECOMPUTE U OPEN LOOP 

C SINCE WNORM USED TO STORE U AND U-ISV FOR CLOSED LOOP SYSTEMS, AND 
C WNORMI USED TO SAVE U-INV OPEN LOOP 
C 

DO 8510 1 * 1 , NS 
DO 8510 J = 1 , N S 

85 10 WNO F.M (I , J) - WNORMI (I ,J) 

CALL MI V (NSQ. WNORM .NS, ODD, D 1 ,D 2 ) 

CALL MODE(WNO&M,FBGC, A A , NS, NC,NS, 3 ) 

985 CONTINUE 

Cr • r ? H E CLOSED LOOP DYNAMICS MATRIX 
DO 160 I * 1 , NS 
DO 160 J * 1 , MS 

SUM * o.5o 
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non 



DO 150 K = 1, NC 

150 SUM=SUn *G (I, K) * F5GC (K, J) 

160 ACL (I,J) =9A (I,J) *SUM 
WRITE /6 , 1701 

170 FORMAT ( * 0* , 1 THE CLOSED LOO? DYN Art ICS MATRIX IS-.*,//) 
CALL RAPRN T (MH,3K,MH, 5, ACL, 4, * (5 (IX, 1 PD13.6) ) *) 

IF (IP . NE. 1 . AND. IR. NE. 3) GOTO 1301 
DO 9 140 1=1, NS 
DO S1U0 J=1 ,NS 
9140 GN ( I , J) = AC L (I,J) 

COOOOOiC-li 3 rt £ C $ a C V v P 3 5 5 

CALL BALANC (NS, NS,GK, LOW, I HIGH, D1) 

CALL ORTHES (NS, NS, LOW ,IHI3H , GN, D2) 

CALL ORTHAN (NS, NS, LOW , IH I3H , G N, D2,SC| 

CALL HOB 2 (NS, NS , LOW -I HIGH, 3 N.CWR # CWI , SC , I ERR) 

I F ( I E RR .NE. 0) CALL EREXI T (NS, GN , I ER R) 

CALL EAL3AK (NS, NS, LOW , IHI33 ,D1, NS, SC) 

C f ^ c-ert c * c c t* $ * vr * c h z * 6 n n*. l c r> a e z b * a c 4 * $ 



70 UNSTABLE CLOSED LOOP 



jd - n 

'( NG . EQ. 
:{ ISET. EO. 
SAD (5.7444 



0) RETURN 
1) GO TO 6 30 

4) ( (G AM (I,J) ,J= 1,NG) ,1=1, NS) 



NORMALIZE AND PRINT CLOSED LOOP SUBOPT. REG. EIGEN SYSTEM 
IWBITE =3 

CALL CNORM (CW R, CW I , SC , NS , I V RITE , N SQ , D DD , D 1 , D2 , WNO R M , V NORMI , F B GC, 
3 AA.NC, NS) 

DO 93 O0 1=1, NS 
IF (C WR /I) . LT. 0. 0) GOTO 9 300 
WRITE (6 , 93 10) 

9310 FORMAT (/// , * PROGRAM TERMINATING DUE 
^SYSTEM* ) 

RETURN 

9300 CONTINUE 

IF(IO-NE.I) GOTO 1801 
DO 9400 1=1, NS 
DO 9400 J=1 ,NS 
9400 W11 (I, J) =SC(I,J) 

CALL MINV (NSQ,W11 ,NS, DDD, D1 , D2) 

1801 NOB =N 0 
IF 

625 IF 

630 CONT^nGe 

I F ( 10 L -FQ. 3) GO TO 9060 
I?( INQ. NE. 0) GOTO 9070 
DO 9050 1= 1 , NG 
DO 9050 J = 1 , N G 
9050 0.(1 , J1 =0.0 

REAt (5, 744 4) (0 (1,1) , 1= 1 , NG ) 

GOTO 9060 

9070 REALf 5. 7444) ( (0 (I , J ) , J * 1 , NG) , I = 1 , NG ) 

9060 WRITE (6 ,6010) 

DO 626 l = 1. NS 

626 WRITE (6,6000) (G A M (I . J) , J= 1 , NG) 

IF( IM . NE. 1) GO TO 8503 
CALL MODE (W NORMI, GAH, A A, NS, NS, NG , 1) 

8503 CONTINUE 

IF f TOL . EQ. 3) RETURN 
Wpf TE (6, bu 1 1) 

DO 627 1 = 1 , NG 

627 WRITE (6, 6000) f 0 ( I , J) , J= 1,N G) 

620 IF( CIO. FO-0) . AfiD. (NG. £w.O) ) GOTO 389 

DO 378 I = 1, NG 

DO 378 J = 1 , NS 
PHOCI.J) = 0. DO 
DO 37 8 K = 1. NG 

* ?«OJI, J) ♦Q (I,F) »GAM(J,K) 

DO 379 J = l'ks 
CQ ( I , J) = 0. DO 
DO 379 K * 1 , NG 

379 CQ(I,J) = CQ(X,J)-GAfl(; f K)» FBO(K,J| 

MOO IrClREG .EQ. 1) GO TO 8 d05 

"•©CALCULATION OF FILTER GAINS: FOPMATIOS OF ESTIMATION HAMILTONIAN 



378 PRO 
DO 



62 



rt tf 
ft 

* F 

ft 

ft 

o 

- HO T ft HI N^BO 

*0 



S3 

!» 

-GM ft Q<*GMT« 

> 
u 

> 



-FT 



^ > F AND FT ARE SAME AS FOR 
CONTROL HAMILTONIAN 
C**Q IS NGXNG STATE DISTURBANCE 
R CO VARIANCE 

30*R IS NOXNO MEASUREMENT NOISE 
BCD VARIANCE 

->*B0 IS NOXNS MEASUREMENT NATRI 
IS NSXNG STATE DISTURBANCE 
DISTRIBUTION MATRIX 
QI ftrtftCftft* <-5CO<; ftfcftft* COC^eiftOf 5 ftftft*C0333 5 ?4C( -Oft'i->*Sft 

1805 DO 9110 1= 1 ,N0 
DO 91 10 J= 1 ,N0 
9110 RC(I,J)=0.0 

READ(5 t 7U«41 (RC (I , I) ,1=1, NO) 

WRITE (6,6052) 

DO 1807 I = 1,NO 



1807 WRITE (6 ,6000) ( RC (I ,J ) . J = 1 , NO) 

8506 IF ( IT r 2 .EQ. 0) GO TO 28 

NOISE TRANSFER FUNCTIONS 



WRITE (6 f 9230) 

92 30 FORMAT (‘ 0‘//,2X. 1 NOISE TRANSFER FUNCTIONS ■ 

C , * THROUGH THE CLOSED LOOP SYSTEM.. 1 ) 

ITF X= 2 
IZE RO =0 

CALL TF (NS . NS, NSQ, ACL , AA,NG , G AM , B K, NO . HO . CM , I Z ER 0 , D, BE , CC , CP , 
* VR.UI, CVR .CWI.SC. JCF,RSS, D1 , D2 , DD D , EPS , I T F2 , I T FX ) 

I F ( I R EG .EQ. 1) KlTURN 
28 CONTINUE 

I F ( I R EG .EQ. 1) GO TO 388 
I F ( IR.LT.2) GOTO 1811 

READ (S. 744 li) ( (FBGE (I,J) ,J= 1,NO) ,1=1, NS) 

GOTO 9320 
1811 CONTINUE 

C^*ft THE MEASUREMENT MATRIX 
C r 9 ( HOT-RIN* % HO===>SC 
DO 30 I = 1 , NO 
DO 30 J = 1 . MH 

30 PRO ( I , J) = HO (I,J)/RC (1,1) 

DO 31 I = 1 , MH 

DO 3 1 J = 1 , M H 
R M ( I ♦ MH , J) = 0. DO 
DO 31 K = 1 , NO 

31 R M ( I ♦ MH , J) = RM (I M H, J) - II O (K, I) • PRO (K , J) 

C? r *GX r '0' GMT= = =>CC 

DO 35 I = 1, NS 



DO 35 J = 1 , NS 
RM ( I , J j = BA (I, J) 
RMiI*MH.J*MH) = -3 A ( J , 



I) 



nni it i.iij on iu 

35 RMjI.JtNS) =C Q ( I , J) 

GO t6 50 

C^^ft G C BACK TO 50 TO SET UP THE FILTER HAMILTONIAN 
C AND CALCULATE THE FILTER GAINS 
60 CALL RGAI N <M, NS , NC, NO B, WR , tf I, X, GN ,GM ,PM, 

1W21 . D 1.CR, Cl, PRO , MHS , D 2 f 
C CHECK EI3VEC 

IF( IDEBUG .EQ. 0) GO TO 58 
WRITE (6.9125) 

CALL hAf PNT (NS, NS, NS, 9, PRO, 4, • ( 9 ( IX, 1 PD 1 3. 6) ) *) 

58 CONTINUE 

IF( IDSTAB -EQ. 1) GO TO 9311 
C 

C NORMALIZE AND PRINT OPT. ESTIMATOR EIGEN3TSTEM 
C 

IWR IT I s 4 

CAL L CNOPfl (CR,CI,?PO, NS , I W fi IT E , S SQ , DD D , D 1 , D2 , V NO r M , - NO F M I , HO, A A , 
* NC.NS) 

93 11 DO 6 1 t = 1 , MH 
DO 6 1 J* 1 , NO 

61 PPO(I,J) * ♦ H 0 ( J , I) /hC ( J , J) 



63 



non n n non 



DO 6 2 I = 1 , M R 
DO 62 J = 1 , NO 
FBGE(I.J) =0. DO 
DO 62 k - 1 ,MH 

62 F53E(I,J) = FBG E (I, J) ♦GN(I. K) c PRO (K, J) 

IF(IDSTAB .EQ. 1) GO TO 9320 
WRITE (6 , 150 1) 

CALL KAPRNT (MH, MH, 

WHITE (6, 1510) 

DO 9312 1=1 ,MH 
9312 X(I,I) = DSORTJGN 



MH, 5,3N, 4 ,* (5 ( 1 X, 1 PD 1 3. 6) ) • ) 






1 , .1 R) 



GAINS • ,//) 



9320 WRITE (6, 1018) 

1018 FOSMAT(*0* . * FILTER STEADY STATE 
DO 63 I = 1 , MH 

63 WRITE J6. 1019) (F6GE(I,J),J = 1 # SO ) 

1019 FOR M AT ( * ' , 2X , 1 P6D1 4. 6} 

C CONFUTE MODAL K MATRIX 

C OPEN LOOP U-INV SAVED IN VNORMI 
IF ( IS . NE. 1) GO TO 9 330 
CALL MODE(WNORMI,FBGE, AA, MR ,MH, NO, 4) 

9330 CONTINUE 

RESET FLAG AND F MATRIX FOR ITERATIVE DESTABILIZATION CASE 

IF{ IDSTAB . EQ. 0) GO TO 9338 
DO 9335 1=1 , NS 
DO 9335 J= 1 .NS 

9335 8 A ( I , J) = BA (I, J) -DSTORE (I, J) 

I R= 2 

9338 CONTINUE 

DO 9340 1=1 , N S 
DO 9340 J=1 ,NS 
SUM =0 . 0 

DO 9350 K= 1 , NO 

9350 S UM =S UM ♦ FBG E ( I ,K) *HO(K,J) 

934 0 FRO (T,J) =BA fI,J) -SUM 
WRITE (6,9361) 

936 1 FOR M A T ( ' 0* . 'THE CLOSED LOOP FILTER DYNAMICS MATRIX IS. .*,//) 
CALL FAPRNTfNS, NS. NS. 5.?FD,4,*(5(1X,1PD13.6)) •) 

IFf IR . LT. 2) GO TO 9500 

r ftf f 

CAL L BALANC (NS, NS, PRO ,LOW f I HIGH , D1) 

CALL ORTHES (NS, NS, LOW , IH IGH ,PP0, D 2 ) 

CALL OR TP AN (NS, NS, LOW , IH I G H . P PD . D2 , GM ) 

CALL HQH2(N5, N$ , LO W r I HIGH, P RO ,C R , Cl , G M , I E R R) 

IF ( I E PR .NE. 0) CALL ER E X I T ( NS, PRO , I E R R ) 

CALL LALEAK (NS, NS. LOW . I H I G H , D 1 , NS , 3 M) 

WRITE (6,9121) 

NORMALIZE AND PRINT SUBOPT. ESTIMATOR EIGEN SYSTEM 
IWR IT E=5 

CAL L CNOFfl (CR,CI,GM,NS, I WPI TE,NSQ,DDD, D1 , D2, WNORM , VNOP MI , HO, A A , 
* NO, NS) 

DO 94 10 1 = 1 , NS 

IF(CR (I) .LT.O .0) GOTO 94 10 

WRITE (6,9420) 

94 20 FOP M A T ( //// , * PROGRAM TERMINATING DUE TO UNSTA3LE FILTER* ) 
RETURN 

94 10 CONTINUE 

GO TO 9501 
q 5 00 IF(IO.EO-O) GO 
9501 DO 65 I = 1 ,NO 
DO 65 J = 1 , M H 
PRO (I , J) = 6. DO 
DO 65 K = 1 , NO 

65 PP0(I,J) * PRO(I, J) ♦RC(I,K) *FB3E(J,K| 

DO 66 I * 1,MH 
DO 66 J = J,MH 



GO TO 389 



eg (i, J) 



DO 



64 



DO 66 K = 1 f HO 

66 CQII'J) = CQ(I, J) -?3GE(I,K) '"PRO (K,J) 
398 CONTINUE 

C-^THE RMS STATE AND CONTROL RESPONSES 



9380 



9705 



97 1 1 



9710 

1501 

1510 
97 15 
1520 



9725 

9720 



97 35 
97 30 



9755 

9750 



976 1 
9760 



9762 



9770 

9360 



I R= IR ♦ 1 

GOTO (9360, 9360,9380, 9380) , IR 
DO 97(35 1=1 ,NS 
DO 9705 J = 1 , N G 
X (I , J ) =0 . 0 
DO 9705 K= 1 r NG 

DO 9710 J=I,NS 
SUM =0.0 

DO 9711 K=1,NG 
SUM=SUM~X(I,K)C»GAM(J,K) 

PRD (T,J) =SUM>CQ (I,J) 

PRO jj ,1) =PnO (I, j) 

CQfl, J) = SU M 
CQ?J, I) = SU M 
W2T (J,J) =GM (I,J) 

V21 (J ,1) =GH (J,lj 

CALL MI N V ( N SQ , W 2 1 , NS , DDD.D1 . D2) 

CALL SCOV (NS,GM,W21,CR,<II, NS ,3M, W2 1 , SR , Cl f PPO,3N) 

WRITE (6 c 150 1) 

FOR MAT ( * O' , //, 2X, 1 THE COVARIANCE OF T HE ESTIMATION ERROR' 
CALL RAFRNT (MH, MH, MH, 5,GN, 4 , ' (5 (IX, 1PD13.6) ) ') 

WRITE (6, 15 10) 

FORMAT!* O' ,/,2X, ' RMS VALUES OF THE ESTIMATION ERROR',/) 

DO 9715 1=1, MH 
X (I -I) - 1 

WRITE l 
FOR MAI 

IF flQ.'EQ.'O) * GO 
DO 3720 1= 1 , NC 
DO 97 20 J= 1 , NS 
SUM = 0 . 0 

DO 9725 K= 1 , N S 
SUM = SUM^FdGC (I, K) «>GN( K, J) 

X (I . J) =SUM 
DO 9730 1=1 , NS 
DO 9730 J= 1 , NS 
SUM =0-0 

IF ( NC. EQ. 0) GO TO 97 30 
DO 97 35 K= 1 , NC 
SUM = SUM ♦G (I ,K) n X (K, J) 

PRD *- -■ J 



(1,1) = DSORTfGN (1,1)) 

RITE (6, 15 2 5) ]xff,I) ,1=1, MH) 
OR MAT ( (5 ( 1 X, 1TD13. 6M ) 

F (10. EQ. 0) GO TO 389 



(I, j) =dg ii, j ) ♦sum 
l scov jrNS,SC,W 1 1 
(NC. EQ. 0) GO TO 9 



CALL 

IF (NC.EQ.O) 

DO 9755 1=1, NC 
DO 97 55 J= 1 t NS 



CWR,CW I ,NS ,GM,W21 ,CR,CI ,P?0, 3 A) 
97 56 



U\J J > J J O— l,t» 

W21 (I - J) =0.5 
DO 97 55 K=1 C N J 
W 2 1 (I , J) = W 2 1 (I,J) ♦PBGC (I, X) *SA(J,K) 
DO 9760 1=1 , MS 
DO 9760 J = 1 , N S 
SUM =0 . 0 

IF (NC. EQ. 0) GO TO 9760 
DC 9761 K= 1 , NC 
SUM»3UM^G (I f K) =*W21 (K, J) 
rno (I f J) = j U M 
DO 37 6 2 1=1 , MS 



(I,J) =Pr5 (I, J) ♦CQ( I, J) ♦PRO (J, I) 

(J ,1) *FRO f I- J) 

' ^ W11,CWR,CWI,N3,SC,yi1,CWR,CWI,?RO,CQ) 



DO 9762 J* I , N S 
PFD 

PRO i«j . i | — i n w i i , o i 
CALL SCOV (N5,SC, 

DC 9770 1=1, NS 
DO 9770 J = I , N S 
G M ( I , J ) =Cj (t,J) -BA (I, J) -3A (J, I) ♦GN (I , J) 

GMjJ, I f = G M (I, J) 

GOTO 9780 

CALL SCOV ( NS, SC, W1 1, CWR ,CW I, NS , SC, W1 1 , CWH , CW I , SQ , Gfl ) 



,//) 



65 



nnn non 



9780 IF (NC.EQ.O) GO TO 20 2 
DO 190 I = 1, NS 

DO 190 J = 1 , NC 
PRO jl ^ = 0. DO 



DO 



1, NS 



191 PRO (I ,3) = p6o(I, J) *GM(I,K) £F53C(3,K) 
190 CONTINUE 

DO 200 T = 1, NC 
DO 200 J = 1, NC 
SC(I.J) = 0.D0 
DO 201 K = 1,NS 

201 SC(I,3) =SC (1,3) + F9GC (I, K) > PRO(K, J) 
200 CONTINUE 

202 IF ( IR EG .EQ. 0) GO TO 9791 
DO 9792 1=1, NS 

DO 9792 3=1, NS 

9792 



9791 

220 



WRITE (6.220) 

FOR MAT f * O' , //.2X. 'THE COVARIANCE OF THE ESTI ■ ATE . . • , //) 
CALL RAPRNT (MH, MH.MH, 5,GB, 4 , * (5 (IX, IP Dl 3. 6) ) • ) 



IF ( I? - GT . 2 ) Gofo $03 
DO b7 I = 1 , M H 
DO 67 J = 1.MH 

67 CQ(I,J) = GN(I,J) +GN(I,J) 
CCNTINU” 



503 

WRITE 
210 FOR tt A 



22 1 



0* f //-2X 'THE STATE COVAPIANC2 MATRIX. 
CALL RAPRNT (MH, MH . MH, 5,CQ, 4,*(5(1X,iPD13.b)) f ) 
IF (NC.EQ.O) g5 t6 2^2 
WRITE (6. 221) 

FOR M A T ( * 0* ,//,1X, V 
DO 230 I = 1 , NC 



,//) 



•THE CONTROL COVARIANCE*,//) 



230 WRITE (6, 231) ' (50(1,3) ,3=1, NC) 

231 FOR M A T ( 1P6D1U .6) 

232 DO 240 I = 1 , NS 

240 CO (1,1) = DSQRT (CQ (I, I) ) 

IF ( NC. EQ. 0) GO TO 25 1 



250 SC 



DO 250 I = 1 , NC 



) = Do 5 RT (SC (1,1)) 



251 WR]f t£ jfc . 26 2 ) 

262 FORMAT ( f 0* , IX,* STATE RMS R E SPON SE * , 23 X , * CO NT POL RMS RESPONSE*, 
io 270 1=1 , NS 

IF (I.LE.NC) WRITE (6 .272) CQ ( I , I) , SC ( I , I) 

27 2 FORM AT (• • r 1 P D 1 5. 7 , 25 { , D 1 5 . 7) 

IF (I.G7.NC) WRITE (6 ,272) CQ (1,1) 

270 CONTINUE 

389 IF ( ITF3 .PQ. 0) GO TO 440 

PORM COMPENSATOR FROM MEAS TO INPUT AND COMPUTE TF 

DO 410 1=1 , NS 
DO 410 J = 1 , NS 
SUM = 0 . DO 
DO 405 K= 1 , NO 

405 SUM = 5 UH* PPG E ( I -K) $HO(K,3) 

4 10 CO (1,3) =ACL(I, J) -SUM 
WRITE (6. 9240) 

9240 FORMAT (* 0* ,//, 21, ’COMPENSATOR TRANSFER FUNCTIONS...*) 

ITF X * 3 
I ZE RO =0 

CALL TP (NS , NS,NSQ,CQ, AA.NO, EDGE, EM, NT. ? PGC , C ■ , I Z E RO. D, BB,CC,C?, 
WF, tfl, CWP ,CWt # SC, JC? , R t S, El , D2,DDD, EPS, ITF3, ITFX) 

440 CONTINUE 

COMPUTE PSD FUNCTIONS OF THE CONTROLLED SYSTEM 

I F ( I ? SD .EQ. 0) GO TO 450 
I P ( I Y 0 . LT. 3) GO TO 444 

CALL PSDCAL (M, MS, RM. X .NC, GW , G V , PSGC , S 0 , H Y , H 0 , HO, F B GE . »G. 

1 GAR, ACL,EA,W?,SfX,Dl, f)2,3CP , 3ES, Q, RC, 3 6, CC, 1 , 1 PSD, 1 NORM) 



66 



oonno 



4 44 
450 



390 

395 



977 1 
9765 



976 3 



9768 

9764 



CALL PSDCAL (M - N S , 8 M - X . NC , GW , G V , FBGC - N 0 , H Y , HI , HO, FBGE, NG, 

1 GAM, ACL.3A,WR, ill, D1, D2 , JCF , R , Q , R C, 9 B , CC , 2 ,1 PSD, I NORM) 

GO TO 450 

CALL PSDCAL (M,NS, BM-X- NC,GW ,GV, FBGC , NO , H Y , HD- HO, FBGE, NG, 

1 GA H, ACL ,B A , W H , tf I ,D 1 , D2,JC?,RES,Q,RC, BB,CC,IYU,lPSD, I NORM) 

IFfISS . *Q. 0) RETURN 
I F ( NC . NE. 0) GO TO 3 95 
DO 390 1=1 , NS 
DO 390 J = 1 , NS 

SSWisft- 3A(I ' J ' 

CALL MIN7 (NSQ, ACL.NS, DDD.D1 ,D2> 

READ(5,7U44) ( W R ( I ) , 1= i , N G) 

WRI TE (b , 97 7 1 ) (tfR(I).I=1,N3) 

FOR M A T ( * 0* , 1 STEADY DISTURBANCE VECTOR 1 , /, 1 0 ( 1 X , 1 PD 1 6. 6 /) ) 

FORMAT?//////,* STEADY STATE VALUES OF STATE VAR- ARE *) 

WRITE (b, 9765) 

DO 9763 1=1, NS 
WI (II =0. 0 
DO 9763 J= 1 , NG 



WI (I) =WI (I) •♦■GAM (I, J)t Wfl (J) 
DO 9764 1=1, NS 
CR ( I ) — 0 - 0 
DO 9768 J= 1 , N S 
CR ( I ) =C R (II - ACL (I, Jp WI (J) 



9766 

9767 



n- r . i x / —v. n ui ntL f kj 

WRITE (b ,6000) CR (I) 

DO 9 7 b6 1=1, NC 
Cl (I) =0. 0 
DO 9*766 J=1 ,NS 
Cl ( I) =CI (I ) +F3GCJI , J) ^CR ( J | 
WRITE ( 6 , 9767) (Cl f " * 



FOR M A^ (/// , * sfSA^Y StAT& CONTROL IS *,///, 1 0 ( 1PD15. 5/) ) 

RETURN 
END 

SUBROUTINE CDIV ( A , B , C , D , E , F) 

IMPLICIT REALMS (A-H,0-Z) 

THIS SUBROUTINE COMPUTES THE COMPLEX DIVISION 

E ♦ F° I = (A ♦ B*I) / (C ♦ D*I) 

T=C^C + DC D 
E=( AOC+B^D) /T 
F= { C- A r D) /T 

RETURN 

END 

SUBROUTINE FA PR NT (N MA X, M , N, L, A, I DIM, FMT) 

REAL*0 A ( N M A X r N ) 

DIMENSION FMT(IDIM) 

NU=L 

DO 20 NL=1 , N, L 
IF (NU.GT. N) MU = N 
DO 10 1=1, M 

10 WRITE <6, FMT) (A (I # J) , J=NL,NU) 

WRI TE]6 f 100) 

100 FOR MAT (' *) 

20 N U = N U ♦ L 
RETURN 
END 

S UB RO UT I NE RG AI N ( M , NS , NC , N 0 B , WR , W I , V P , G N , W 1 1 , TCB , 

1W21 / LT,C,CI-CT / MHS,MT) 

IMPLICIT REAL-8 (A-H,0-Z) 

DIMENSION W R ( M ) , • I ( M) , VF (M, M) ,3N(NS,NS| 

DIMENSION Vi 1 (NS, NS) ,TCS(M , M) , W 5 1 (NS, NS) ,L7 (NS) , T 
DIMENSION C (N S) , C I ( NS ) , CT ( N S , NS ) 

K * 1 

K? * 1 
KN = 1 

NRZEV * 0 
NC? ZE V = 0 
IF ( X. GT • A) 



M T (NS) 



10 



GO TO 200 
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CHECK FOR EIGVAL AT OP NEAP J- 3 MEGA AXIS TO INCLUDE IN S-L EIGSYS 
TORN FIRST ONE POSITIVE AND S2COND ONE NEGATIVE 



EIG VR^DAES (WR (K ) ) 

IFfEIGVR .GE. 1.5-10) GO T3 40 
IrJSI (K) ) 40- 30,40 
30 NRZEV = NRZEV+1 

I F ( NR ZE V .GT. 1) GO TO 35 
VP.(K) = EIGVR 
GO TO 75 

35 WR(K) = -EIGVR 
WRITE (6-9000) 

9000 FOR M AT ( ' O’ # 1 EU LER-LA GRANGE EQ0ATI3NS HAVE 
* ' On NEAR ZERO. * /) 

GO TO 110 



A PEAL EIGENVALUE AT’ 



40 NCPZEV = NCPZEV ♦ 1 
IFfNCPZEV .GT. 1) 
WR(K) * EIGVR 
WR ( K ♦ 1 ^ = EIGVR 



GO TO 45 



GO TO 

4 5 WP.(K) = - El G VR 

W R ( 1) = -EIGVR 

WEI TE (6. 9010) 

9010 FOR tt A T ( ' 0* - * EULER-LA GRANGE un.L, n v_ 

O 'EIGENVALUES AT OP NEAR THE J-OME3A AXIS.') 
GO TO 120 

100.50,50 



EQUATIONS HAVE A COMPLEX PAIR OF \ 



48 IF (WR (K) ) 1 00,50. 5C 

50 IF ( W I (k([ 80, 75,50 
EIGENVECTOR 



FOR REAL EIGENVALUE, POSITIVE 

75 IF ( NO B . EQ. 0) GO TO 78 
DO 76 J - 1 , J! 

76 TCB (J,KP) = VF(J,K) 

78 KP = KPO 

K=K ♦ 1 
GO TO 10 

EIGENVECTOR FOR COMPLEX El GEN V ALUE , PCSITI V E PEAL PART 

00 IF( NOB . EQ. 0) GO TO 83 
DO 81 J = 1.M 
FR = VF(J.K) 

FI = -VF(J,K*1) 

TCB ( J , K P) = FR + PI 
8 1 TCB (J .KP*1) = FR- FI 

83 KP = KP*2 
K - K ♦ 2 
GO TO 10 

100 IF(WI(K)) 120-110.120 

: EIGENVECTOR FOR REAL EIG ENV ALO E , N EG ATI VE REAL PART 

1 10 C (KN) = WR (K) 

Cl (KN) « MI (K) 

IF (NOB. NE . 0) GO TO 96 
K NS = K N ♦ N S 
DO 9 5 J = 1 , M 

95 TCB (J.KNS) = VF (J,K) 

96 KN = KNO 
K=K ♦ 1 

GO TO 10 

EJGENVECTOP FOB COMPLEX EIGEN V ALUE, NEGATIVE SEAL PART 

120 PR = WR ( K) 

HI = WI (K 

/KN) = F.fc 
(K N* 1) = RS 

[KN) = R I 
Cl ( K N ♦ 1 ) = - RI 

IF (NOB.NE. 0) GO TO 122 
KNS - K N ♦ N S 
DO 121 J * 1,B 
FP = VP/J.K) 

f: = - v f (j ,*♦ i) 

TCB ( J , K N S ) = FR*PI 

121 TCB ( J , K N S ♦ 1) * pc - 

122 KN * KN ♦ 2 



* FR - FI 
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GO TO 321 



K = K >2 
GO TO 10 

200 CONTINOE 

IF (NOB. NE. 0) 

PORHATION OF W 1 1 
DO 300 I = 1 , NS 

DO 300 J = 1.NS 
Mil (I,J) = TC3(I,J*NS) 

300 CT(I,3) = W 1 1 (t,J) 

FORMATION OF W21 
DO 320 1=1 , NS 
DO 320 J=1 , NS 

3 20 W21 (I , J) = TCB (I + N S , J* NS1 

321 IF (NOB. EQ. 0) GO TO 323 
DO 322 I = 1 , N5 
DO 322 J = 

W21 (I , J) = 

322 W1l]l,Jj 

323 CONTINU 
INVERT W11 



0 - 

1 = 



1, NS 
-TCB II, J) 
TCB (1+ NS , J ) 



NS3 = NS*'NS 

CALL HINV (NSQ.W1 1 , NS, DETC, LT, MD 



CALCULATE THE RGAIN 
DO 325 I L= 1 , N 5 
DO 325 JL= 1 , NS 
GN(IL.JL) = 0. DO 
DO 325 KL= 1 , NS 



, n o , uu 

MATRIX 



325 



5999 



10 
1 5 



20 



GN(IL^JL^=GSjIL^JL^W21 (IL, KL)*W1 1 (KL, JL) 



DO 5999 
DO 5999 



I * 1,NS 
J = 1 , NS 

RE? U f! N - W11(J ' I » 

END 

SU3FOUTINE HINV (NSQ.A.N.D, L,f1) 
IMPLICIT R E AL<*8 (A-H,0-2| 

DIMENSION A (NS0) r L (N) ,H (N| 

DOUBLE PRFCISION A , D, 31 £ A, H OLD 
NH -N*N 
D= 1 -0D0 
N K = - N 

DO 60 K = 1 , N 
NK= NK ♦N 
L (K) =K 
H (K } = K 
KK=NK*K 
BIG A= A ( K K) 

DO 20 J = K,N 
12= (J-1) 

DO 20 I = K, N 
I J= I Z ♦ I 

IFf DABS(BIGA)- DAES(A(IJ))) 15,20,23 
BIG J = A(IJ) 

h]k| =J 

CONTI NUE 



INTERCHANGE ROWS 
J = L { K) 

IF(o-K) 35,35,25 
25 K I = E - N 

DO 30 1=1, N 
K 1= K I ♦ N 
HOLE = -A <KI) 

JI=KI-K^J 
A 

30 A 

INTERCHANGE COLURKS 
35 I =H (K) 



(KI) 


= A (JI) 


iJI) 


* HOL U 
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IF(I-K) 45,45,38 
38 JP=S* (1-1) 

DO 40 J= 1 , N 
JK= KK ♦ J 
JI=JP*J 



40 



HOL D = - A (JK) 
A (JK) = A (Jli 
A (J I ) = nOLD 



DIVIDE COLUMN BY MINOS PIVOT (VALOE OF PIVOT ELEMENT IS 
CONTAINED IN BIGA) 



45 IF f BIGA) 48,46,48 

46 D=0 - 0 DO 
RETURN 

48 DO 55 1 = 1. N 

rF(I-K) 50,55,50 
50 I K = N K ♦ I 

55 

FEDUCE MATRIX 

DO 65 1=1, N 
IK=NK+I 
HOL I = A ( IK) 

I J= I - N 
DO 1 5 J =1 , N 
I J= I J ♦ N 



IF (I-K 
6 0 IF " 
6 2 KJ 



F 

F J-Ki 
J=IJ-I< 



60,65,60 
62,65,62 

♦ K 

A (I J) =HCLD^ A (KJ) ♦ A ( IJ ) 

65 CONTINUE 

DIVIDE ROW BY PIVOT 

K J= K- N 
DO 75 J= 1 , N 
KJ= KJ+N 

IF(J-K) 70,75,70 
70 A (KJ) =A (KJ) /BIGA 
75 CONTINUE 

IFODUCT OF PIVOTS 

D=D r BIGA 

REPLACE PIVOT BY RECIPROCAL 

A (K K) = (1 -0D0) /BIGA 
00 CON 71 NU E 

FINAL ROW AND COLUMN INTERCHANGE 



100 K = ( K - 1 ) 

I F ( K ) 150, 150 ,1 05 

1=1. jk) 

if ( i - k 



1 05 



) 120,120,108 

-il 

- 1 ,» 



1 0 8 JO* NO ( 

J?.~ x * r (I 
DO 1 1 0 J 
J K = JQ ♦ J 
HOL P = A (JK) 

J 1= J it *J 
A (JK) =- A (JI) 

no a ( j i $ =hc3ld 

1 20 J=M (K) 

IP(J-K) 100,100, 1 25 

125 K I = K - 5 

DO 130 1=1 , S 
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K 1= K I ♦ N 
HOL D= A (KIJ 
JI=KI-K+J 
A (KI) = -A (JI) 

130 aJjiS =HOLD 
GO TO 100 
15 0 K=0 

RETURN 
END 

SUBROUTINE SCOV ( N L , W L , W LI , VL 1 , V L2 . NR , W R , W P I , V R 1 , V F2 , 
R EA L c 8 VL1 ( NL) , Vl2 (NLJ , wL f N L, NL), WLI ( NL , NL) , X (NL, NR) , 
VR1 (NR) ,VR2(NR) , WR (NR, NR) ,WRI (NR, NR) 

PEAL38 A ,3,0, D, K 1 , K2 , K3 , K4 
100 DO 50 I = Ml 
DO 50 J = 1 , N H 
X (I . J) =0. 

DO 50 11 = 1 , NL 

5 0 X (I x J)=X (I, J) *WLI (1, 1 1) *Q(I I, J) 

52 1 = 1 , r * 



DO 52' 1 = 1, NL 
DO 52 J = 1 , N R 
‘ I.J) =0. 

51 J J = 1 , NR 






5 1 QJI , J|^=Q (i; J) *X (I, JJJ ^WRI (J , JJ) 



10 , 1 1 , 1 0 
20 , 21,20 



(J» SOVL2 (] 


[ ) * B) 


/D 


(JKB + VL2 (] 


d f cj 


/D 



52 CONTI 
1=1 

1 3 IF (VL2 (I) ) 

10 J = 1 

14 IF ( V R 2 ( J ) ) 20,2 

20 A = v L 1 (I) ♦ V R 1 ( J) 

B=-2.*VL2(I) 4, VR2 (J) 

C = A* * 2*VL2 fl) 2* VP2(J) * *2 
D=C r ^2-BC*i 
K 1 = A 1 ” C/D 
K 2= - ( VR2 
K 3= - ( VR2 
K4=-A*>B/1 
11=1+ 1 
J 1 = J ♦ 1 

X (I , J) = ♦ K 1*Q (I, J) *K 2*0 (I, JI) ♦ K3-Q 
X (I . J 1) =-K2>Q I,J UkIhMi, JI -K4*>0 
X (I 1 , J) =-K3*0(I,J) -K40Q (I, JI) ♦ K1*Q 
X (I 1 1 JI) *♦ KU*Q(I, J) -K 3*Q (I, J 1) - K2-Q 

GO TO 22 

2 1 A = V R 1 rj) *VL1 (I) 

B = A 2 * VL2 (I) *'2 

K1= A/B 

K2= VL2 ( I ) /B 

X (I , J) =K 1*0 (I ,J) -K2*0 (I*1,J) 

X (I ♦ 1 , J) =K 2^Q (I,J)»kH(M,J) 

J=J ♦ 1 

22 IF ( J . LS. NR) GO TO 14 
1 = 1*2 
GO TO 12 



I 1 , J) *K4?Q (I 1 , JI) 
I 1 , J *K3 < 0 1 1 , JI) 
I 1 , J *K2' 0 (I 1 , J 1) 
I 1 , J) ♦KV'j I1,J1 



1 1 
15 

30 



31 

32 



J = 1 

IF ( V P 2 (J) ) 30, 31 , 30 

A = V H 1 fJ) *VLl (if 
B = As*^ 2*VR2 (J) ^2 
K1= A/B 
K2=VR2(J)/B 

x (i , j) = ki> g (i, j) -K2*g (i, j*i ) 

X (I , J* 1) =K2 -'0 (I , J) *K 1^0(1, J *1) 
J = J ♦ 2 
GO TO 32 

X (I ,J) =0 (I, J) /(V?1 (J) *VL1 (I ) ) 
J=J ♦ 1 

IF (J.LE.NR) GO TO 15 
1 = 1*1 



12 IF (I . LS.NL) GO TO 13 
DO 40 1=1, NL 
DO 4 0 J = 1 , S3 
Q ( I » J ) = 0 . 
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DO 40 11=1 , NL 

40 Q ( J I i J) i 2 1 0jIr J l ♦ WL(I r II) >X(II,J) 

DO 42 J = 1 ' N E 
X a, J)=0. 

DO 41 JJ=1,NR 

41 X (I ,J) = X (I ,J) +Q (I, JJ) ~WR (J r JJ) 

42 CONTINUE 
RETURN 
END 

SUBROUTINE MODE ( W NORM , G, GN 3 Rfl , N S , N 1 , N 2 , I C'C N) 

WNORM TRANSFORMATION MATRIX U OR U-INV 

NS NO. OF STATE 

NC NO. OF INPUTS OR OUTPUTS 

ICON CONTROL FLAG TO INDICATE WHICH TRANSFORMATION 

0 = MODAL G 

1 = MODAL GAMMA 

2 =• MODAL H 

3 = MODAL C 

4 = MODAL K 

5 = CONTROL EIGENVECTOR MATRIX 

6 = MEASUREMENT EIGENVECTOR MATRIX 

IMPLICIT REALMS (A-H.O-Z) 

DIMENSION UNO BH (NS # NS ) . G (N 1 . N 2) , GNORM (>l 1 f N 21 
&500 FOR M A T ( * O' , //, 2 X , • MODAL CONTROL DISTRIBUTION MATRIX • //) 

5501 FORMAT?' 0* , // , 2 X , ' MODAL PROCESS NOISE DISTRIBUTION MATRIX • //> 

b57 0 FORMAT?* 0* ,//.2X, 1 MOD AL MEASUREMENT SCALING MATRIX* //I ' //} 

65 8 0 FORMAT (///#' * wnnii "fi'.’Tnnr ^itvq i p r . r ' ' i 

6584 N ‘-*' 

6586 run an t i ’ v • / t uwa.iu PwtiLm 
6000 FORMAT * * , (2 X, 1P6 D 1 4 . 6j ) 

6590 FORMAT? *0* 'MODAL FILTER ST 
DO 50 1=1, N1 
DO 50 J = 1 , N 2 
50 GNORM (I, J) =0. 

IPO I N T= I CO N ♦ 1 

GO TO (75.75. 250, 250, 75,250,250) ,IPOENT 
75 DO 100\M,n5 
DO 100 1=1 , NS 
DO 100 K = 1 , NS 

100 GNORM (I, J|=GNORM (I, J) ♦WNORM (I . K ) & G (K , J) 

GO TO ( ^1 05 , 150, 250,2^0, 160) , I POINT 
105 WRITE (6 . 6500) 

110 DO 120 1=1 .NS 

120 WRITE (6,6000) (GNORM (I ,J) ,J* 1,N2) 

RETURN 

150 WRITE (6.6501) 

GO TO 110 
160 WRITE (6.6590) 

GO TO 110 
250 DO 260 J=1 , NS 
DO 260 1=1, N1 
DO 260 X =1 , NS 



FORMAT?///, * * , * .HE MODAL CONTROL GAINS APE:** //) ' 

FOR M A T ( ' 0 * ,/, * CONTROL EIGENVECTOR MATRIX. • //{ 
FORMAT?* O' ,/* MEASUREMENT EIGENVECTOR MATRlJ.. ' .// 
FORMAT » •. 1P6D14 .61 1 ''' 



//) 

EADY STATE GAINS • ,//) 



26 0 GNORM (I, J) = GNORM (I, J) ♦ 3 (I ,K) F WNO? M ( K , J) 

GO TO ( 261.26 1, 261 ,2to2, 261 , 263,264) ,IP<3 i&t 
26 1 WRITE (6.6570) 

GO TO 270 
26 2 WRITE (6,6580) 

GO TO 270 

26 3 WRITE (6.6584) 

GO TO 270 
264 WRI TE (6 , 65 86) 

270 DO 27$ I = 1 . N1 

27 5 WRITE (6,6000) (GNORM (I ,J) ,J* 1 , NS) 

RETURN 

END 

SUBROUTINE CNORM ( W Z , W Y , V EC , NS , I V PI72 , SC Q , HDD , D1, D2, WNORM, WNOF MI, 
8 HO, CM, K 1 , N2) 



WZ<I> 



REAL PA BT OP I-TH EIGEN ? A LU E 
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WY(I) COMPLEX PART OF I-TH EIGENVALUE 

VEC MATRIX OF RIGHT EIGENVECTORS STORED IN PEAL FORM 

FROM HQR2 

NS NO. OP STATES 

I WRITE FLAG TO CONTROL FORMATS FOR DIFFERENT EIGHENSYS TEMS 

WNORK NORMALIZED MATRIX U OF RIGHT EIGENVECTORS STORED 

BY COLUMNS IN REAL FORM 

W NO R MI O-INVERSE 2* C CNGU GATE OF LEFT EIGENVECTORS 

STORED BY ROW IN REAL FORM 
NSQ,DDD,D 1, D2 - ARGUMENTS PASSED TO MINV 



IMPLICIT REALMS (A-H.O-Z) 

REAL'" 8 FIELD, COMMA , ScMCOL.R IGHT f FMT 

nr»«Tvc*T/''k\i i - *r / m c* \ ’j v ci ur*'*/vic* v: c- 1 



9030 
9040 
9050 
9060 
907 0 
9080 
9090 
9100 
91 10 
9120 
9130 

1 1 



r r-a x. u n i iu 

DIMENSION WZ (NS) , WY (NS) , VEC (NS . NS) , W NORM ( NS # NS) 

. W NORM I (NS, NS) , STORE (6) , D 1 ( NS) , D2 ( N S) , F MT ( 1 4 ) , HO (N 1,N2) , 
CM(N1 r N 2) 

DATA FIELD/5HE12. 5 / e COMM A/5 H *, • , /,5EMCOL/5H, • : • ,/ 
,RIGKT/1H) /, FMT/6H (IX, IP , 13MH /. S EM EN D/4 H , • : •/ 

FOR MAT 'O' , 'OPEN LOOP EIGEN VALUES.. • I 

FOR MA T ( 1 0* # * CLOSED LOOP OPTIMAL REGULATOR E IGEN V A LUES . - • ) 

FOR MAT i *0* , ‘CLOSED LOG? SUB0P7IMAL REGULATOR EIGENVALUES..*) 
FORMATi'O* ,* CLOSED LOOP OPTIMAL ESTIMATOR EIGENVALUES..*) 

FOR M A T ( * 0 * , * C LOS ED LOOP SUBOPTIMAL ESTIMATOR EIGENVALUES..*) 



) 



i v n . . n x i f v. l, u ^ r .uuuriitinL x x . . n a v i n j c, i 

FORMATiV/'O*. 'RIGHT EIGENVECTOR MATRIX..'//) 

FCRMAT(//'0' , 'OPEN LOOP LEFT EIGENVECTOR MATRIX..' 

FORMAT?//' 0* , 'CLOSED LOOP OPT. REG. LEFT EIGENVECTOR MATRIX..') 
FORMAT///' O' , 'CLOSED LOOP SUBOPT. REG. LEFT EIGENVECTOR MATRIX..' 
FORMAT//'©', 'CLOSED LOOP OPT. FILTER LEFT EIGENVECTOR MATRIX. .1* ) 
FORMAT (//' O' , * C LOS ED LOOP SUBOPT. FILTER LEFT EIGENVECTOR MATRIX. 

fIb MAT (461,' (',F10.7,')*J(',P10.7,'| ') 



NORMALIZE COMPLEX EIGENVECTORS BY LARGEST ELEMENT 



KK= 0 
LF= 0 
LC= 3 

TO ^9 9 K = 1 , N S 
IF(KK.EO.I) GOTO 991 

IF (DABS (HY (K)) . LT. 1. D-101 GO TO 999 
LC= LC ♦ 1 
EMAX = 0 . DO 
DO 9°7 I = 1 , NS 

CMO 0= VEC (I . K )S«»2*VFC (I.K*1)*-*2 
IF (CMOD-EM AX) 997,990,990 
990 EMAX = CMOD 

997 CONTINUE 

VMR = V EC ( M , K ) 

VMI = V EC f M . K ♦ 1 ) 

DO 980 1=1, NS 
VR = VEC (I , K) 

VI - VEC jl . K* 1) 

VEC ?N = (VF> VMR ♦V I> VMI) /EMAX 
VFC IN* (-VRC- VNI4 VI* VMR) /EMAX 
WNORrt (I,K) = V ECR N 
wso RM (I, V* 1) = VECIN 
980 CONTINUE 
KF> 1 

GOTO 999 

991 

999 CONTINUE 

NORMALIZE REAL EIGENVECTORS 3T THE TOTAL LENGTH 
DO 1000 f=1,NS 

:f( dabs (wy (K) ) .ge. i.d-io) goto iooo 
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998 



996 



995 

1000 



LE= LR ♦ 1 



RESOD = 0. DO 
DO 996 1=1- NS 
RENOD=VEC(I,K 
RNDD = DSQRT (REflOD) 
DO 995 1=1 , NS 
R VEC = VSC (I, K) /RNOD 
WNORM (I, K) = Rv EC 
CONTINUE 
CONTI NOE 



♦REflOD 



GO TO (520 .530, 540, 545,550) 
520 WRITE (O-9030) 

GO TO 5b 0 
530 WRITE (6 r 90 4 0) 

GO TO 560 
540 WRITE (6. 90 5 0) 

GO TO 560 
545 WRITE (6, 9060) 

GO TO 560 
550 WRITE (6,9070) 

560 KK=0 



N PS T W -0 

NFflTV=1 

DO 568 1=1, NS 

I F ( KK . EQ- 1) GO TO 567 

IF (DABS (WY (I) ) -GT. 1 - D- 10) 

C PRINT OUT KC flORE THAN 6 WORDS, 



, I WR ITE 



KK= 1 



NOT SEPARATING 



IF(NPRTW . LT. 5 .OP.(NPRTW .EQ. 5 -AND. KK 

FMT (NFflTW* 1) = RIGHT 

WRITE (6 , FflT) (STORE (J) ,J = 1, NPRTW) 

N PR TW =0 



COflPLEX EIGVAL 
. FQ. 0) ) GO TO 561 



NFM T W = 1 

56 1 NPRTW = N PFT W ♦ 1 
N Ffl T W = N F MT W ♦ 1 
IF ( KK . EC- 1) GO TO 562 
STOKE (NPRTW) = WZ(I) 

FAT (NFflTW) = FIELD 
NFrtTW = NF HTW ♦ 1 
FKT (NFF.TW) = SEN COL 
GO TO 568 

56 2 STO PE (NPRTW) = HZ (I) 

FflT(NFNTW) = FIELD 
FMT (NFflTW* 1) = CO fl fl A 

STORE (NPRTWO) = WY(I) 

FAT ( N F fl T W ♦ 2 ) = FIELD 

FAT JN FflT W* 3 1 = SEflCOL 
NFfliW = NFflTW ♦ 3 
NPRTW = NPRTW ♦ 1 
GO TO 568 

567 KK= 0 

568 CONTINUE 

FflT (NFflTW) = SEttZND 
FAT NFflTW^ 1) =* RIGHT 

WRITE (6 , FflT ) (STORE (J) ,J = 1, NPRTW) 
WRITE (6.9080) 

CALL ft A f RNT (N S, NS , NS , 6 , W NOR fl, 4, 1 <6 
. 5) 

GO TO 578 



570 

57 5 
578 
580 

590 

600 

6 1 0 



I n f 4 . 

■TE it, 

TEj6 t 
. L RAf 

CALL RAPRNTjNS^ NSLNSjb,WVDRfl’a' • 
GO TO (576. 570- 570, 5/5-5 75) ,*WRI 
CALL flflDE(WNORfl,HO,C.A,NS,N1 , N2, 5 



(6(U, 1PD13.6)) •) 
|6 (1PG12.6) ) ') 



CALL ttODE fW NORN , HO- Cfl, NS - N 1 -N2, 6) 

GO TO (580 -590, bOO, 610, o20) ,IWF.ITE 
WRITE (6 , °0 9 6) 

GO TO 630 



■ PITS (6-9100) 
GO TO 630 
WRITE (6,91 10) 
GO TO 630 
WRITE (6,9120) 
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GO TO 630 
620 WRITE (6-91 30) 

C SAVE 0-INV OPEN LOOP IN WNORAI 
6 3 0 IF ( I WRI TE . GT. 1) GO TO 100 5 
DO 510 1=1 , NS 
DO 510 J=1 , NS 

510 W NO r ft I ( I , J ) = W NO R ft ( I - J ) 

CALL ftINV (NSQ.WNORrtI, NS, DDD , D1, D2) 

CALL RAPRNT (NS,NS,NS,6,WN0RHI,4,* (6(1X,1?D13.6)) 1 ) 

RETURN 

1005 CALL ftINV(NSQ,WNORtt,NS,DDD, D1 ,D2) 

CALL RAPRNT (NS, NS, NS, 6, NOR fi, 4, • (6 ( IX , 1 PD 1 3 . 6) ) • ) 

RETURN 

END 

SUBROUTINE TF (N , NS, NSQ.A , A A , ft,B, BN, L, C, CM, IF Dr W, D, 3B,CC,CP, 

* EVE.EVI.PH. PI, SC, JCF, RES, D1 , D2,DDD, EPS, ITF, ITFX) 

If.? LICIT REALMS (A-H, 0-2) 

Dirt FN SI ON A (N,N) , A A (N ,N .B( N, ft) , Bft (N, ,1) ,C(L,N) ,Crt(L,N) , D ( L , ft 1 . 

5 BB (N) ,CC fN) ,CP (N , EVk (N) , EV I (N) , PR (N) , PI (N) , SC (N , N) , JCF (N > , 



r 

SAVE 



COrtPUTATt OON ON OL A> 



_ __ . ND CL SYS WITH rtODAL WORK DONE IN CPTSYS 

IFfITFX . EQ. 1) GO TO 50 
I F j ITFX .EQ. 2) GO TO 5 

CALL POLES (N, Nft , A , A A, M / B,L f C,?R,PI,Dl ,D 2, JCF, SC) 

CORPUTE NODAL MATRICES FOR nESI&UES 
5 DO 10 1=1, N 
DO 10 J= 1 , N 
10 AA(T.J) = SC(I,J) 

15 DO 26 1*1, L 
DO 20 J = 1 , N 
CR(I.J) = 0.D0 
DO 20 k=1,N 

20 CK ( I , J) = Crt(I f J) ♦ C (I.K) > AA (K, J) 

CALL HINV(NSQ,AA,N,DDD,D1,D2) 

DO 30 1=1, N 
DO 30 J*1.fl 
5ft (I . J) = 0. DO 
DO 30 K = 1 , N 



j = (! , J ) ♦ AA (I, K) > B (K, J) 



DO 

3 0 3ft (I, 

5 0 CON 

DO 100 1 = 1 , ft 
DO 100 J = 1 , L 
IF( ITF - ME. 3) 

"CALL ZEROS ( I, J, IFDP W, N , N ft, A , A A , ft, 3 , L, C , D , 3 B , CC , CP , EV R , 2 VI , D 1 , D2 , 

* EPS) 

J F ( ITF .NE. 2) CALL R ESI D ( I , J , N , JCF , ft , 5 fl , L , Cft , PR , PI , R ES , D B , CC , 1 ) 
100 CONITNUE 
R FT U R N 
END 

SUBROUTINE CHECK (EPS, NC, K3 , NO) 

DOUBLE PRECISION EPS 

COR RON /PROG/ IOL. INQ.IQ.IP ,ISS, III, IT FI , ITF2,ITF3 , IFDFW, IE, ID STAB 
" I DEBUG. IS ET , I REG, IPS D, I YU , I NORA 
: SET ftODAL ANALYSIS WHEN OL EIIENSYS OR OL TF REQUESTED 

I F ( I ft .EC. 1 .AND. IOL . EQ. 0) IOL=1 
IF ( IOL .?0. 3 .OR. I T FI .NE. 0) Ift=1 
I CHECK TO SEE IP H AATPIX INPUT 

I F ( NO .NE. 0 .OR. IOL . S E. 2) GO TO 25 
■ PI TE (6 , S000) 

9000 POR ft A T (//* H - RATRIX ftUST BE INPUT, I. 

STOP 

25 CONTINUE 

TRANSPEE FUNCTION CHECKS 
I E= 6 



NOB 



;t. o*) 



IF(I2 . EO. 0) 
ZP$ = 10.:** (-IE 
OPEN LOOP TP 

IPjlTFl - EQ. 
WRITE (6, 9000] 



) 

0 .OR. NC .NE . 0) GO 70 50 



3 ) 

9000 PORRA ?(//• INPUT(G) RATRIX RUST 5E 8 ZQO ESTEP (I- E . 
•TO CORPUTE OPEN LOOP T . F. •) 



NC .NE. 0) 
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ODD on 



STOP 

C COMPENSATOR TF 

50 I F ( IT F3 .EQ. 0) GO TO 100 

IF? IR EG .EQ. 0 .AND. (NC . N E. 0 .AND. NG . NE. 0} ) GO TO 100 
WRITE (6 ,91U0) 

9100 FOR tt A T ( //' REGULATOR AND FILTER SYNTHESIS MUST BE REQUESTED IN • 
, 'THE SAME RUN TO COMPUTE COMPENSATOR T. F. • ) 

STOP 

100 CONTINUE 
C NOISE TF 

I F ( IT F2 .EQ. 0) GO TO 150 

Ir] NG . NE. 0 .AND. NC .NE. 0) GO TO 150 

WRITE (6, 9200) 

9200 FOR M A i (//* NOISE T. P. CALCULATED ONLY WHEN REGULATOR DESIGNED * 

* , 'AND GAMMA INPUT, I. E. NG . NE. 0*) 

STOP 

DESTABILIZATION RESTRICTIONS 
150 IF ( IDSTA 5 . EQ . 0) GO TO 200 
I F ( NC . EO. 0) GO TO 200 
IF?NG .N£. 0) I R EG = 1 

WRITE (6,9300i 

9300 FOR tt A T (//' DESTABILIZATION OPTION DESIGNED FOR A REGULATOR OR ', 

• 'FILTER BUT NOT BOTH SIMULTANEOUSLY') 

IF ( IR EG .EQ. 1) GO TO 200 

STOP 

200 CONTINUE 
PSD INPUT 

I F ( I PSD .EO. 0) GO TO 300 

IF ( I P SD .L?. 0 .OR. I FSD . 3 T. 3) GO TO 250 
IF ( I Y U . LT. 0 .OB. IYU .3T. 2) GO TO 250 

IF ( I NOP M .LT. 0 .OR. INORM .GT. NG ♦ NO ) GO TO 250 
GO TO 275 
250 WRITE (6, 9400) 

9400 FOR tt AT ( ' *rttbv*4** INCONSISTENT PSD INPUT FLAGS **£»*****•) 

STOP 

275 I F { I P EG .EQ. 0 .AND. NC .NE. 0) GO TD 300 
WRITE (6 , 9500) 

9500 FOR MAT ( ' A REGULATOR AND FILTEP MUST BE RESIDENT «, 

1 'TO COMPUTE THE PSD OF A CONTROLLED SYSTEM!*) 

STOP 

300 CONTINUE 
h ET U R N 
END 

SUB POUT I NE POLES (N, Ntt , A, AA, H, B, L , C , EV 3 , E V I , D 1 , D2 , J CF , SC) 

IMPLICIT REALMS (A-H,Q-Z) 

DIM EN SI ON A (N,N) , A A (N,N| , B( N,tt) , C (L,N) , EVR ( N ) , EV I ( N) , D 1 (N ) , D 2 ( N) , 
** JCF (N) , SC (N,N) 

DO 1 1=1, N 
DO 1 J= 1 f N 

1 AA (I. J) = A (I.J1 
Qri *C’~njr’V O'hahrr.t. of 

CALL EALANC (NM, N, A A .LOW, I HI GH,D1) 

CALL 0R7HES (NM, N, LOW, I HIGH, AA, D2) 

CALL ORTFAN (Ntt, H. LOW. THIGH, AA,D2,SC) 

CALL H C R 2 ( N M , N , LG V , IH I GH . A A , I VB , E V 1 , 5 C , I E? R) 

I F ( I ERR .NE. 0) GO TO 110 

CALL PA LEAK (Ntt, N.LOW, I HIGH, D1,N, SC) 

WRITE (6, 101) 

101 FOR M AT (/// , 23H TF DENOMI VAT OR EIGENVALUES:) 

DO 2 1=1 ,N 

2 WRITEjb, i02) EVRm f Z7I(I) 

102 FORMAT (/,2l,3H (,Fl3.b,aH) ♦J(,F13.6, 1!l) ) 

RETURN 

110 WRITE (6.9000) 

9000 FOR M AT ( ' FAILURE IR HQH2, CALCULATING POLES') 

100 RETURN 
END 

SUB POUT I NE ZEBOS ( K 1 , 1 2, I FDP W , N, Ntt , A , A A , M , B, L, C, D , 5 B , CC , C? , EVP , EVI 
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D 1 , D 2 , EPS) 

IMPLICIT REALMS (A-H, 0 - 2 ) 

N) ,AA (N,N( ,B(N,M) ,C(L,N) ,D(L,M) ,BB(N) ,CC(N) ,C?(N) , 



DIMENSION 
0 EVR (N) , 2 
DOUBLE PRECISION 
DO 1 1 = 1 , N 



h D dllk 



D 2 (N) 

S 



EB 

CC 



1 AA 
101 



[I] 

J= 1 # 
J) =ij 



. . = C f K 2 , 1 ) 

DO 1 J= 1 ,N 



A A ( I , J) = A f I , J) 

WRITE ( 6 , 10 1 ) K 1 ,K 2 
FORMAT (///, 17 H IF I 



A T ( /// , 1 7 H TF FOR INPUT N0.,I3,15H AND OUTPUT NO.,13,': 1 ) 
IF ( IF DF W . EQ. 0) GO TO 2 
H= D ( K2 , K1 ) 

IF(DABS (H) .L2.EPS) GOTO 2 
J J= N 
GO TO 5 

2 NN=N-1 

DO 3 1=1, NN 
H=S CL (N , BB , CC) 

CALL CCOMP (N, NM , AA, CC ,CP) 

IF(DABS (H) .GT.EPS) GO TO 4 

3 CONTINUE 

H=5 CL (N, BB , CC) 

WRITE?6,105) H 

102 FORMAT (//, 5X, 26HNO FINITE ZEROS. 

GO TO 100 

4 J J= N- I 

5 WRITE (6, 103) JJ ,H 

103 A ‘ 



TF G A I N= , F 1 3 . b) 



FORHAT(/,3X,20h6hDER OF NUHERATOR =,I3,9X,8HTF SAIN=,F13.6) 

CALL ACOKP (N. Nfl.AA, 3B,CC,H) 

J* ± l iittdirrrttjucivftaci 

CALL BA LAN C (NM, N , A A . LOW , I H I GH ,D 1 ) 

CALL ORTHES (NM. N, LOW, I HIGH, A A. D2) 

CALL HQR (NM,N .LOW. I HIGH. AA, EVh, EVI, IERR) 

I F ( IE RR .NE. 5) GO TO 110 

WRIT? (6 , 104) 

104 FORMAT(/,3X ,57H N UME RA TOP EIGENVALUES (INCLUDING EXTRANFOUS ZERO V 
1LUES) :) 

DO 6 1=1. N 

6 WRITK(6,105) EVR (I), EVI (I) 

105 F0RMAT(/,4X (' , F 1 3 . 6 , M ♦ J ( * , FI 3 . 6 , M ') 

100 RETURN 

110 WRITE (6. 9000) 

9000 FO° M At ( * FAILURE IN H QR CALCULATING TRANSFER FUNCTION ZEROES’) 
RETURN 
END 

SU3POU7INF ACOMP(N, KF. , A, B, 0 , H) 

REAL^H A. fa. C, H 

DIMENSION A (NM,N) ,B(N) ,C(N) 

DO 1 1=1, N 
DO 1 J= 1 , N 

1 Ajr^,J^=A (I , J) -fa (I) >C{J) /H 
END 

SUBROUTINE CCOM P ( N , NM , A , C, 0 C) 

PFAL"8 A.C.CC 

DIMENSION A (N.M, N) ,C (N) ,CC(H) 

DO 1 1=1, N 
CC(I) =0. 

DO 1 J= 1 , N 

1 CC(I) =CC (I) ♦C (J) *A (J, I) 

DO 2 1=1. N 

2 C (I ) = CC ( l) 

RETURN 

END 

FUNCTION SCL (N, B, C) 

R F A L r 8 B.C.sdL 
DIMENSION 3(N),C(N) 

SCL =0 . 

DO 1 1=1, S 
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1 SCL =S CL + C ( I) -B(I) 

RETURN 

END 

SUBROUTINE RESI D ( K 1 , K 2 . N , JC ?, M, 3M , L, CM , PR , PI , RES , B3, CC, IPT) 
IMPLICIT REAL^8 (A-H,0-2) 

DIMENSION JCF (N) , BM (N , M) , CM (L, N ) , PR (N ) , PI (N) f BES ( N) , SB (N) , CC ( N) , 
C PRT (4) 

DATA 5N'/8H^SIN (B» T/, R 1/3 H 
DATA ZERO/0. DO/, ri/4H*'T**/, BLANK/8H 
DATA ED/ 1 H) / 

: TEMPORARY MOD TILL JCF IS CALCULATED 

DO 5 1=1 r N 
5 JCF (I) =0 
: TEMPORARY MOD 

IF ( IPT .EQ. 1) WRITE (6 ,9000 ) 

9000 FOn MAT (//, 3X r ’RESIDUES AT THE POLES: * /# T 1 6 , 1 ? OLE S',T41, 

* R E S I D U E S' ,/,T9, ‘REAL (A) 1 , T26, • I MAG (3) ' ) 



* / . R2/8 H EXP (A-T) / 

/,cs/‘ 



CS/8 H*COS ( B^T/ 



BB 
10 CC 



DO 10 I = 1 , N 



= BM (I, K1) 



ill = CM(K2il) 

LOCP THROUGH THE POLES 



100 



1=0 

1=1*1 



I F I I .G7. N) GO TO 500 



C COMPUT 



IF (JCF 
I F ( D A B 






.EQ. 1) 



SIMP 
RES (I) = CC 
RESjI + 1) =CC 
IrfIFT .EQ. 
PRT (1) = EL 
PRT ?2) = R 
IFfPI (I) . 

PRT 
PRT 
WRI 



fl. 3 l : 

ITE (6, S 



1) GO TO 300 

Pi (in At, i.d-io) go to 200 

LE COMPLEX POLE RESIDUES AND PRINT BOTH 

- si 

EL ANK 
R 2 

‘ “ P FT ( 2) = BLANK 



wU nr LL a 4 u Ll r lo i ju l j a n u r 
(I)»BB(I) ♦ CC ( I ♦ 1 ) * BB (I ♦ 1 ) 

(I)*B5(T*1) - CC (1*1 ) *BB (I 
0 GO TO 110 



(I) . EQ. 0.D0) 

■ = cs 

= ED 
, 90 20) 

i=m 
PRT (3) =SN 
WRI TE (6,9020) 

GO TO lOO 
1 1 0 I = I ♦ 1 
GO TO 100 
200 CONTINUE 

C COMPUTE SIMPLE REAL POLE RESIDUE 
RES (I) = CC (I) *BB (I) 

IF f IPT .EO. 0) GO TO 100 
PRT (1) = HI 



PR (I) , PI (I), RES (I) , (PRT (J) ,J=1,4) 
PR (I) ,PI(I) ,RES (I) , (PRT (J) , J=1,4) 



PRT 2 = 
PRT (3 = 



R2 
BLANK 



PRT (4)= BLANK 
r Jr (b . 9fl 



’ H ( I ) ,ri (I) ,RES(I) 



WRITE (6, 90 20) 

GO TO 100 

LOOK AHEAD TO DETERMINE SIZE OF THE JORDAN 
300 K=1 

K T = N - I 

PO 310 J=I,KT 

IF^JCF(J) .EQ. 0) GO TO 320 
320 CONTINUE 



(PPT(J) , J = 1 r 4) 
BLOCK 



DA BS (PI 



IF i 

COMPUTE R P 
K = 1 



tuny 



-LT. 1 .0- 10) GO TO U 00 
COMPLEX POLE AND PRINT OUT 



ALL FOUR 



FES (I ) =CC ( I ) *38 (I) ♦CC (I* 1) > BB (I* 1) ♦CC (I* 2) ~33 (1^2) ♦CC fl* 3) *3R ( I* j 
FES (I^1)=LC(I)*BB(I^1)-CC(I^1) i B3(I) ♦C.C (I*2)*c3(I*3)-cC(I*3) * 

X b B (I +2) 

(I ♦ 2) =CC (I ) * BB (I ♦ 3 ) ♦ccr 

jl43)=CCil)»BB(IOi-CC( 

IPT .Eg. 0) GO TO 340 



FES (I ♦ 2) =CC 
RFS 
IF< 

PFT ( 1 ) — F 1 
PPT (2) 



I * 3B (I ♦ 3 ) *CC (I * 1 } * B 3 (I ♦ 2) 
-CC ( I ♦1) f ’?B(I^2| 



»R2 
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IFfDABS (PR (I) ) .GT. 1.D-10) 
PR T ( 1) = ELANK 
PRT (2 = BLANK 
330 PRT (3) =CS 



GO TO 333 



PRT ju =ED 

WRITE (6, 90 2 0) PR (I) f PI (I) f R ES (I) , (PRT (J) , J = 1 f 4) 

90 2 0 FOR MAT (/, 4 X , • (* , F 1 3 . 6 , ' ) + J ( 1 f FI 3 . 6 , * 1 * , 4X, ■ (« ,F1 3. 6, ■ ) ' ,3.18, A 1) 

90 3 0 FORMAT j/,4 X (•,F13.6,M+J( , ,F13.6, , | ' , 4X, 1 (■ ,F1 3.6, • ) • , A4 ,12 , 2X, 



.LT. 1.D-13) P3T ( 2) =3LANK 

PR (I), PI (I) , RSS(I) ,?RT(1) ,K, (PRT ( J j ,J=2,4) 



* 2A8.A1) 

PRT (3 ) =S N 
1 = 1 + 1 

WRITE (6, 90 2 0) PR (I) , P I (I ) , 3 ES (I) , (PRT ( J ) , J = 1 , 4) 

PRT (1) = T1 
PRT (2 j =R2 
IFfDAES (PH (I) ) 

PRT (3) = CS 
1 = 1 + 1 

WRITE (6, 9030) 

PRT (3) =SN 
1=1 + 1 

WRITE (6.90 30) PR (I) , PI (I) ,RSS(I),PRT(1),K, (PRT (J) ,J=2,4) 
GO TO 100 
34 0 I = I + 3 
GO TO 100 

C COMFOTS REPEATED REAL POLE RESIDUE AND PRINT OUT *LL K OF THEM 
400 CONTINUE 
KT= I + K- 1 
NN= 0 

DO 4 2 0 J=I,KT 
NN= N N ♦ 1 
RES ( J ) = ZERO 
DO 410 JJ=J,KT 

4 10 RES (J) =PES (J) +BB (JJ) PCC (JJ- NN^I) 

420 CONTINUE 

IF ( IPT .EQ. 0) GO TO 4(40 
N N= 0 

S T 1 
= R2 

= BLA N K 
. ... , i = BLA NK 
DO 430 J=I.KT 
WRITE (b, 9030) 

4 30 N N = N N + 1 

GO TO 100 
44 0 I = KT 

GO TO 100 
50 0 'CONTI NUE 
RETURN 
END 




P R ( J ) ,PT (J) , 3 ES (J) , PRT ( 1) , NN, (PRT ( J J) , J J = 2 , 4) 



SUBROUTINE BALANC (NP1, N,A, LOW, ISH, SCALE) 

NM f I GH,LOW,IEXC 
IX 

LOGICAL NOCONV 

DATA PADIX/24 21 0000 00 000 03 3 0/ 

52 * RADIX * RADIX 
K * 1 

L 3 N 
GO TO 100 

:::::::::: IN-LINE PROCEDURE EOF ROW AND 

COLUMN EXCHANGE :::::::::: 

20 SCALE (•) 3 J 

IF (J .cC. M) GO 70 5 0 

DO 30 I 3 1, L 
F 3 A (I, J) 



INTEGER I / J,K / L / M f N,JG / 
REALMS A (NM, Nf f SCALc( N[ 
REALMS C,P,5,R,3,82 f FAb 
REA L® 8 DADS 
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c 

c 



c 

c 



30 

40 

50 

80 

100 

110 

120 

130 

140 

150 

170 

180 

190 



A (I, J 
A ' 
COST 






= A (I,M) 
= F 



DO 40 I = K, N 
F = A (J, 1) 

A (J,I) = X (M,I) 

AJW.I = F 
CONTINUE 

GO TO (80, 130) , I EXC 

:::::::::: SEARCH FOR ROWS ISOLATING AN EIGENVALUE 
AND PUSH THEK DOWN :::::::::: 

I? (L . E^. 1) GO TO 280 

FOR J=L STEP -1 UNTIL 1 DO -- 
DO 120 JJ - 1, L 
J “ L ♦ 1 - JJ 



DO 110 I = 1 , 
IF (I . EQ. 

if [A(j,n 

CONTINUE 

M - L 
I E XC = 1 
GO TO 20 
CONTINUE 



) GO TO 1 10 

NE. O.ODO) GO TO 120 



GO TO 140 



K = K ♦ 1 
DO 170 J = K, L 
CO 150 I = K, L 



SEAPCH FOR CO LU fl NS ISOLATING AN EIGENVALUE 
AND PUSH THEM LEFT :::::::::: 



IF (I . EQ. J) GO TO 1 50 
IF (A (I , J ) .NE. O.ODO) GO TO 170 
<TINUE 



CO NT IN 

r. = K 
I E XC = 2 
GO TO 20 
CONTINUF 

:::::::::: NOW BALANCE THE SU&MATRIX IN ROWS 

DO 180 I = K, L 
SCALE(I) =■ I.ODO 

:::::::::: ITERATIVE LOOP FOR NORM REDUCTION 

NOCCNV = .FALSE. 

DO 270 I = K, L 
C = O.ODO 
P = C.ODO 



ro l 



DO 200 J = K , 
IF (J .EQ. 



L 

I) 



GO TO 2 00 



200 



c = c . 



p * p 

CONTINUE 



DAES l 



IF (C .ED. 0. 
G * R / ? A Dt X 
F * I.ODO 



GUARD AGAINST ZERO C OR R DOE TO UNDERFLOW 
O.CDO .OR. B . EQ. O.ODO) GO TO 270 





£ * C 


♦ R 






210 


IF (C 


. GE. G) 


GO TO 


220 




F « F 


* RADII 








C « C 


> 52 








GO TO 


210 






220 


G = r 


> RADIX 






230 


IF (C 


. LT. G) 


GO TO 


240 




F * F 


/ RADIX 
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C 



240 



C 

250 

C 

260 

C 

270 

C 

C 

280 



C 



C = C / B2 
GO TO 230 

:::::::::: NOW BALANCE :::::::::: 

IF ( (C ♦ R) / F .GE, 0. 9 5 DO * S) GO TO 270 

G = T.0D0 / P 

SC ALE (I) = SCALE (I) * P 

NOCGNV = .TRUE. 

CO 250 J = K, N 
A (I,J) = A (I, J) * G 

CO 260 J = 1, L 
A <J,I) = A ( J , I) f T 

CONTINUE 

IF (NOCONV) GO TO 190 

LOW = K 
IGH = L 
RETORN 

:::::::::: LAST CARD OF 3ALANC :::::::::: 

END 



C 



C 



C 



C 

C 

C 



C 



C 

C 



C 

C 

C 



SUBROUTINE ORTHES (Nfl, N,LOW, IGH, A , OHT) 



J,fl,N,II,JJ.LA,MP,NM, IGH , KP 1 



INTEGER I,J,fl,N .II,JJ.I 
REALMS A (NM,N) ,0RT (IGH) 
REAL«8 F.G.H, SCALE 
REA L^ 8 DSQhT, DABS, DSIGN 



LOW 



LA “ IGH - 1 
K PI = LOW ♦ 1 

IF (LA . LT. KP1 ) GO TO 200 

DO 180 K = KP1, LA 
H « 0.0D0 
GRT(fl) = O-ODO 
SCALE = 0.0D0 

:::::::::: SCALE COLUMN (ALGOL TOL THEN NOT NEEDED) 
CO 90 I = tt. IGH 

90 SCALE = SCALE ♦ DA ES ( A ( I , H-1 ) ) 

IP (SCALE • EQ* 0.0 DO ) GO TO 180 
flP = fl ♦ IGH 

:::::::::: FOR I*IGH STEP - 1 UNTIL fl DO — :::::::: 

DO 100 II = fl. IGH 
I - .IP - it 

ORT(I) = A ( I ,fl- 1) / SCALE 

H * H ♦ ORT (I) 8 ORT(I) 

100 CONTINUE 



1 10 



G * -DSIGN (DSQRT (H) , OPT ( fl) ) 

H = H - ORT(rt) © G 
ORT(fl) * ORT (fl) - G 
::::::: FORfl (I-(U*UT)/H) * A :::: 

DO 130 J = fl, N 
F = 0.0D0 

:::::::: FOR I * IG K STEP -1 UNTIL fl 

DO 110 II * fl # IGH 
I * .IP - It 

F - P ♦ ORT ( I) • A (I,J) 
CONTINUE 



DO -- 



F = P / H 

DO 120 I * fl, IGH 

120 A (I, J) = A (I, J) - ? * ORT (I) 

130 CO NT I NOE 
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C 

C 



140 

C 

C 

150 

C 

160 

C 



180 

C 

200 

C 



F = 0,000 

:::::::::: FOR J = IGH STEP -1 UNTIL H DO -- 

DO 140 JJ = M, IGH 
J = HP - JJ 

F = F ♦ ORT (J) - A (I,J) 

CONTINUE 

f = f / a 

DO 150 J * S, IGH 
A <1, J) = A (I, J) - F ' ORT (J) 

CONTINUE 

OKT(M) = SCALE ♦ ORT(K) 

A(H,fl-1) = SCALE * G 

CONTINUE 

RETURN 

:::::::::: LAST CARD OF ORT HES 

END 



SUBROUTINE CRT? AN (Nfl, N,LOW , IGH, A, ORT, Z) 

INTEGER I- J,N. KL, HH. HP. NH, IGH, LOW, a PI 
REAL* 6 A (Art, IGH) ,ORT(l6H) ,Z (NH, N) 

REAL** 8 G 



60 

80 



INITIALIZE Z TO IDENTITY MATRIX :::::: 
DO 80 I = 1, N 

DO 60 J « 1. H 

z(i,j) = o.5do 

2(1,1) = 1 • 0 DO 
CONTINUE 

KL = IGH - LOW - 1 
IF (KL . LT. 1 ) GO TO 200 

: : : PO« HP=IGH-1 STEP -1 UNTIL LOW* 1 DO 

DO 140 MH * 1 , KL 
HP * IGH - MM 

IF (A (fl P , H P- 1 ) . E0- O.ODO) GO TO 140 

H P 1 = H P ♦ 1 



DO 100 I = H P 1 * IGH 
100 ORT ( I ) = A (I, HP-1) 

DO 130 J = HP, IGH 
G = O.ODO 



DO 110 I = HP, IGH 
110 G = G ♦ ORT (if s z (I, J) 

:::::::::: divisor delow is negative of h formed in orthfs. 

DOUBLE DIVISION AVOIDS POSSIBLE UNDERFLOW :::::: 
G * (G / ORT (TP)) / A<r.F, HP-1) 

DO 120 I * HP. IGH 

120 Z(I,J) * Z(I,J) ♦ 2 > ORT (I) 

130 CONTINUE 
C 

luO CONTINUE 
C 

200 RETURN 

C :::::::::: LAST CARD OF o?r RAN :::::::::: 

END 

C 
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c 

c 






c 

c 



c 

c 



c 

c 



SUBROUTINE HQR2 (NM , N, LOW ,13 H ,H, W R, WI, 

INTEGER I,J,K,L,M, N,EN,II,JJ,LL, .'IK, N.\ . Uv 
X IGH ,ITS -LOW ,MF2, ENM 2,IERR " rN •• 

REA L 1 * 8 H (NM.N) , WR (N1 , WI (N) , Z (NM, N) 

REA L- 8 P e Q#K,S r T f W.X c Y.HA, S A, VI ,VR,ZZ 
REALMS DSQKT, DABS, DSIGN 
INTEGER KINO 
LOGICAL NOTLAS 
COflPLEX^ 16 Z3 
COX PL EX*" 16 DCMPLX 
R EA L rt 8 DREAL, DI MAG 

:::::::::: STATEMENT FUNCTIONS ENABLE 

IMAGINARY PARTS OF DOUBLE PRECISION 
DREAL (Z3) = Z 3 

DIM AG (Z3 ) = (0. 0D0,-1 . 0D0) * Z3 

DATA KACHEP/Z34 1000 00 000 00 0 00/ 

IERR = 0 
NORM = 0 . 0 DO 
K = 1 

:::::::::: STORE ROOTS ISOLATED BY B\m v . 

AND COMPUTE MATRIX NORM :: 2 V . T . 

DO 50 I = 1, N • 



“ ACHE? 



'IION OF REAL AND 
LUMBERS 



DO 40 J = K, N 

40 NORM = NORM ♦ DA3S(H(I,J)) 
K = I 

IF jl .GE. LOW .AND. 



C 

C 



SO CONTI 



WR ft) = H (I. I) 
wi (if = o. 060 
TINUE 



I . LE. IGH) 70 



EN = IGH 
T = 0.0D0 



C 

C 



IF 


(EN 


. LT 


ITS 


= 0 




NA 


= EN 


- 


ENM 


2 = 


N A 



SEARCH FOR NEXT EIGENVALDr^ 
LOW) GO TO 340 



LOOP POR SINGLE SMALL SUi'-oiir.w 

FOB L= F N STEP -1 UNTIL LO* po ELS 



70 DO 80 LL = LOW. 2N 

L = EN ♦ LOW - LL 

IF (L . EQ. LOW) GO TO 100 

S = DABS (H (L-1. L-1 ) ) ♦ DABS(H(L,Ui 

IF (S . EO. 0. 0D0) S = NO PM 

IP (DABS (H (L, L-1) ) .LE. MACHEP * Si rn ~ 

80 CONTINUE ' 1,0 TO 100 

:::::::::: FORM SHIFT :::::::::: 

100 X = H (EN ,EN1 

IF (L . EC. ^N) GO TO 270 
Y = H ( N A , N A ] 



-WENT 



W = H (EN.NA * H (NA. EN) 

IF (L . Eg. N A 1 GO TO 280 
*“ .Eg. JO) GO TO 1000 



C 

C 



IF 


(L . EO. 


IF 


(ITS .E 


IF 


(ITS .N 



T * 



ITS .s£. 10 .AND. ITS . NE. 20) 

FORM EXCEPTIONAL SHIFT 

T ♦ X 



V?.T9. no 



DO 120 I = LOW. E S 
120 H (I , I) * K ( I, I) - X 

S- DABS^H (F.N^NA) ) ♦ DABS (H (N A , ENM2) ) 

Y 3 X * 

W = - 0. 4 37 5 DO C S » S 
130 ITS * ITS ♦ 1 
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c 

c 

c 



:::::::::: LOOK FOH Two CONSECUTIVE SMALL 
SUB-DIAGONAL ELEMENTS. 

FOR M=EN-2 STEP -1 UNTIL L DO - 
DO 140 MM = L, ENM 2 
M = ENM2 ♦ L - MM 



ZZ = H ( M ,M ) 
R = X - ZZ 



= Y - ZZ 
= (R 3 S 



W) / H (M *1 r M) ♦ H ( M # M ♦ 1 ) 
R • S 



140 



= fi (M ♦ 1, M* 1) - ZZ - 

= H (K ♦ 2 , M* 1 ) 

= DABS (P) ♦ D A 3S (Q) ♦ DAES(R) 

= P / S 

Q - Q / S 

R = R / S 

IF (M .EQ. L) GO TO 150 

IF (DABS (H (K, M-1) ) 41 (DA BS (Q) + 

K '* (DABS (H (M-1, M-1)) ♦ DAPS(ZZ) 

CONTI NUE 



DABS(R)) -LE. KACHEP f DABS(P) 
♦ DABS (H ( M ♦ 1 , M ♦ 1) ) ) ) GO TO 150 



150 MP2 = M ♦ 2 
C 

DO 160 I = MP2, EN 
H ( I r 1-2) = 0.0D0 

IF (I .z.Q. MP2) GO TO 160 
H(I,I-3) = o.obo 
160 CONTINUE 

C DOUBLE Q R STEP INVOLVING ROWS L TO EN AND 

C columns a TO EN :::: :::::: 

DO 260 K = M, NA 

NOTLAS = K . NE. NA 
IF (K .EQ. M) GO TO 170 



P = H (K, K-1) 

Q = H (K^ 1, K- 1) 



170 



100 
1 90 



R = 0 . 0 DO 
IF (NOTLAS 
X = DABS (P 
IF (X .EO. 

P = P / X 

C : jj / l 

S = DSIGN(DSQRTf ?vp^ 
IF (K . EQ. M) GO TO 
H(K.K-I) 

GO TO 190 



R = H( K*2,K- 1) 

♦ DAES (Q) ♦ DABS (R) 

0.0D0) GO TO 260 



R’' 1 R) , P) 



-S • X 
M) H(K,K-1) * -H(K,K-1) 



IF (L . NS 
P * P ♦ S 
X = P / s 
Y » Q / S 
ZZ = P / S 

9 » 9 / p 

? = S / p 

::::::: ROW MODIFICATION 

DO 210 J * K . N 

P = H (K, J) ♦ Q o H (K ♦ 1 



200 

210 



H K,jf ♦ 0 

IF (.NOT. NOTLAS) <20 
P ♦ R * H [K*2, J) 

2 , J | = H ( K + 2 , J) - 

H 1 , J ) « H ( K* 1, jj - 



P = ' P ♦ R 
H (V* 2 , J } 



H ( K, J f « H(K,J) 
T : NtIF 



CONTINUE 



t 6 J ^ 

P * 
P » 

X 



00 



ZZ 

Y 



J = KINOfEN.KO) 

:::::::: COLUMN MODIFICATION :::: 

DO 230 I = 1 , J 

P = X*H(I f K)*T* H (I. 1) 

IP (.NOT. NOTLAS) GO TO 220 



P ♦ ZZ * H (I,K*2> 



220 

230 



(I, K*2) 


= H (I, K+ 2) 


(it i 


* H (I. F>l[ 


luv ■ 


H ( I r K) - F 



R 

Q 
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ACCUMULATE TRANSFORMATION? ; ; . 

DO 250 I = LOW , IGH 

P = X 3 Z (I t K) ♦ T * 2(1, F> 1) 

IF (.NOT. NOTLAS) GO TO 240 





Z i 


[I, K + 2) 


ta 4a ^ 4a \ JL , i‘ ’ - 

= Z (I, K*2) 


240 


z 


(I, K4-1 J 


= Z (I, K+ii 




z < 


I,K) = 


Z ( I , K) - £> 


250 


CO NT] 


CNUE 



F * H 
P > Q 



260 



CONTI NUE 
GO TO 70 



270 



200 



ONE ROOT FOUND 
H(EN.EN) = X ♦ T 
WR(EN) = H (EN.SN) 

WI(EN = O.ODO 
EN = NA 
GO TO 60 

:::::::::: TWO ROOTS FOUND 
F s iy - X) / 2.0D0 
0 = P 6 P «■ w 
22 = DSQRT (DAES (Q) ) 

H(EN,EN) = X ♦ T 
X = H (EN , EN) 

H (NA, NA) = Y ♦ T 
IF (Q . LT. O.ODO) GO TO 323 
:::::::::: REAL PAI P. 

ZZ = P «■ DSIGN(ZZ,P) 

VR(NA) = X ♦ ZZ 
VR(EnS = WR (NA) 

IF (ZZ . NE. O.ODO) WR (EN) * 

WI ( N A ) = O.ODO 
W I ( E N ) = O.ODO 
X = H (FN,NA) 

S = DABS^X) ♦ DABS (22) 

o = \lf / S 

h = DSQRT ( r°P+Q*Q) 

P = P / R 

Q * Q / H 

:::::::::: ROW MODIFICATION ::::::::: 

DO 290 J = N A . N 
ZZ = H ( N A , J) 

H ( N A , J) - Q * ZZ ♦ P HfEN,Jl 

H [ EN , J) = Q o H(EN,J) - P * ZZ 

CONTINUE 

:::::::::: COLUMN MODIFICATION 

DO 300 I = 1. EN 
ZZ = M ( I # N A ) 

H ( I , N A ) = Q e ZZ ♦ P * H (I# EN) 

H jl. EN) = Q * H (I, EN) - M zi 

TINUE 

:::::::::: ACCUMULATE TRANS FORMATIONS 

DO 310 I = LOW, IGH 
ZZ = Z ( I , N A) 

Z ( I , N A ) = Q * ZZ ♦ P 5 Z ( I , E N ) 

ZJI, EN) * Q * Z (I, EN) - P * ZZ 



W / ZZ 



290 



310 



300 CONT 



320 



330 



340 



L I 1, L 

CONTINUE 
GO TO 330 

:::::::::: COMPLEX PAIR : : : ::::::: 

WR(NA) = X ♦ P 
WR(ES) = X ♦ ? 

W I ( N A j = ZZ 
WI ( EN) = -ZZ 
EN = FN M2 
GO TO 60 

ALL ROOTS POUND. 2ACKSU bZ TITUtp 
VECTORS OF UPPER TRIANGULAR ?(j»* 
IF (NORM . EC. O.ODO) GO TD 1001 
:::::::::: FOR ZN=N STEP -1 UNTIL 1 do -- 
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600 



6 10 
620 



630 



640 



DO 800 NN = 1 , N 
EN = N ♦ 1 - NN 
P = WR(EN) 

iiva 8 ?! 

IF (Q) 710- 600, 800 
:::::::::: REAL VECTOR :::::::::: 

fl = EN 

H(EN.EN) = 1.0D0 

IF (NA . EQ. 0) GO TO 800 

:::::::::: for I=£n- 1 STEF -1 UNTIL ' - >c . 

DO 700 II = 1, NA 
I = EN - II 
W = H (I ,1) - P 

R = H (I -EN) 

IF (tt .GT. NA) GO TO 620 

DO 6 1 0 J = H, N A 
R = R «■ H(I,J) “ H (J, EN) 

IF (WI(I) .GE. 0. 0 DO) GO TO 6 3C 
zz = w 
S = R 
GO TO 700 

n * i 

IF (WI(I) .NE. 0. 0 DO) GO TO 6a: 

IF (W . EQ. 0.0D0) T = NACHEP * sor, 
H(I,EN) = -R / T 
GO TO 700 

SOLVE REAL EQUATIONS 



C 

C 

C 

C 



6 S 0 
700 



7 1 0 

720 

730 



X = H (1, 1*1) 

Y = H (I* 1 - 1) 

0 = (WP (if - P) • (WR /I) - P) 
T = ( X ft S - ZZ e R) / Q 
H(I,EN) = T 



IF ( D A I 3 S ( X ) .LE. DABS (ZZ) ) GO 
H (I* 1 ,E li) = (-R - W * T) / X 

GO TO 706 

H (!♦ 1 , E N) = (-S - Y ' T) / ZZ 
CO NT I NU E 

END PEAL VECTOR 
GO TO H00 

CoRPLLX VECTOR :::::::::: 

tt = NA 

:::::::: LAST VECTOP COMPONENT CH:~ 

EIGENVECTOR flATRIX IS TFT* 
IF (DADS (H (EN, NA) ) . L E. DABS ( H ( N A . 

’ H (t N , N ‘ 



- *1 (I * 
650 



WI (I) 



H (NA, NA) 

H (NA. r 
GO TO 



. - - 0 / H (LN.NA) 

H { N A , EN i - - (H ( E N, EN) - ?) / H(FS. 
730 



; :’n ! I ?J aiNARY 50 T 
rN ) ) I GO *fo' 7 20* ’ 
V M 



Z3 = DC.1PLXf0.0D0, -H(NA, EN)) / D;*«v lf!) , wi M 
H ( N A , A 1 = iREAL(i3) * ( N A , N A) - P, Q) 



!I ( N A, EN ) = DI1AG(Z3) 

H ( £ N , N A = 0.0D0 
H EN,ES * 1.0D0 
ENK2 = NA - 1 
IP (EN12 . EQ. 0) GO TO BOO 
:::::::::: POF I^EN-2 STEP -1 UNTIL * 

DO 790 II = 1- SNO 2 
I * NA - It 
V = Hjl-I) - P 
RA = O.DDO 
SA = H ( I , EN) 

DO 76 0 J * fl, NA 

PA = PA ♦ H (I,J) * H (J , NA) 

5 A * S A ♦ H(I,J) * H(J,En( 
760 CONTINUE 



DO 



IP (WI(I) .GE. O.OD3) 

zz = w 



GO TO 7^ -• 



HAT 
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non n on noon non o n on 



770 



700 



785 



R = R A 
S = SA 
GO TO 790 
M = I 
IF 
Z 3 



<WI (I) 

= DC Mi 



, NE. 0. 0 DO) GO TO 780 



MPLX (-RA,-SA) / DCMPLX(W,Q) 



H 






DREAL 


(Z3) 




S r toi 


f 90 


DIM AG 


(Z3j 




SOLVE COMPLEX EQUATIONS 



[♦ 1,1 

» R ( I ) - P) > (S 8(1) - P) f V 

«R]l) - P\ * 2. 0D0 * Q 

__ .... .EQ. 0.0D0 .AND. VI .SQ. 0. t 

(dABS(W) ♦ DABS (Q) ♦ DABS ( X) ♦ DAB^(Y) ♦ DA3S(ZZ)) 

Z3 = DCMPLX (XCR-ZZ^RA +QPSA, X^S-ZZ^SA-Q^RA) / DCMPLX ( V? , VI) 
" ** Z3) 



MI (I) 5 »I (I) 

.0D0) VR = MACH EP 



NORM 



= DREAL 
H(I,EN) = DIMAG 
IF (DABS ( X) 

H (I* 1 , N A) = (-R A - 

H(I*1 # EN| = (-SA - 

GO TO 790 

Z3 = DCMPLX (-R-Y*H 



.Z3l 

. LE. DABS (ZZ) 



H (!♦ 1 ,N A) = D PE AL 
H ( I ♦ 1 ,EN) = DIM AG(Z3 



Z) ♦ DABS (Q) ) GO TD 785 
H(I,NA) ♦ 0 * H (I, EN) ) / 
H(I,SN - 0 * H(I,NA / 



r# NA) , -S-Y^H (I, EN) ) / DCMPLX ( ZZ , Q) 



790 CONTINUE 

:::::::::: END COMPLEX VECTOR :::::::::: 

800 CONTINUE 

:::::::::: END BACK SUBSTITUTION, 

VECTORS OF ISOLATED ROOTS ::::::::: 
DO 04 0 I * 1, N 

IP (I .GE. LOW .AND, I . LE. IGH) SO TO 8U0 



820 



DO 820 J * I, H 
Z (I,J) = H (I,J) 



8U0 CONTINUE 

:::::::::: MULTIPLY BY TRANSFORMATION MATRIX TO GIVE 

VECTORS OF ORIGINAL FULL MAThIX. 

POE J = N STEP -1 UNTIL LOW DO -- ::::::::: 

DO 880 JJ = LOW , N 
J - N ♦ LOW - JJ 
M = .UNO (J,IGH) 



DO 880 I = LOW, IGH 
ZZ - 0, 0D0 



DO 860 K = LOW. M 

860 ZZ * ZZ ♦ Z (I , K ) * H ( K, J) 



Z (I, J) 

380 CONTINUE 



ZZ 



GO TO 1001 

:::::::::: GET ERROR -- NO CONVERGENCE TO AN 

EIGENVALUE AFTER 30 ITERATIONS :: 

1000 IERE * EN 

1001 RETURN 

:::::::::: LAST CARD OF HQR 2 :::::::::: 

END 



C 



C 



SUBROUTINE BAL3AK (NM, N , LOW , IGH, SCALE, M, Z) 

INTEGER I, J,K,M,N,II, NM,IGH , LOW 
?FAL*8 SCALH(M) ,Z (NM, M) 

REA L* 8 S 

IF (M . EQ. 0) GO TO 200 
IF (IGH .EQ. LOW) GO TO 120 
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C 



100 

110 

120 



130 

140 

200 



DO 110 I * LOW, IGH 
S = SCALE (I) 

:::::::::: LEFT HAND EIGENVECTORS ARE SACK TRANSFORMED 

IF THE FOREGOING STATEMENT IS REPLACED BY 
S= 1 - 0D0/SC ALE (I) . :::::::::: 

do ioo j = i, a 
Z (I, J) = Z (I , J) * s 

CONTINUE 



FOR I=LOW-1 STEP -1 UNTIL 1, 

IGH ♦ 1 STEP 1 UNTIL N DO — :::::::: 
DO 140 II = 1, N 
I = II 

IF (I -GE- LOW .AND. I . LE. IGH) GO TO 140 
IF (I . LT. LOW) I = LOW - II 
K = SCALE (I) 

IF (K . EQ. I) GO TO 140 

DO 130 J = 1, M 
S = Z (I,jf 
ZfI,J) 33 fe(K,J) 

Z?K.J = S 
CONTINUE 

CONTINUE 

RETURN 

:::::::::: LAST CARD OF BALBAK :::::::::: 

END 



SUBROUTINE HQR (NM, N ,LOW, IGH ,H,WR , WI , I ES R) 

INTEGER I, J ,X,L,M, N.EN.LL, M M, NA f NM, IGH, ITS, LOW , M P 2 , E NM 2 < 
RFAL*8 H (5 k. N) , WR (N) , WI ( N| 

REAL- 8 P,0, T, W.X, Y,ZZ, NORM, MACHEP 
RSAL*8 DSOHT, DABS, DSIGN 
INTEGER MlNO 
LOGICAL NO X LA S 



DATA MACHEP/Z34 100000000000 00/ 

I ERR =0 
NORM = O.ODO 
K = 1 

STORE ROOTS ISOLATED BY BALANC 
AND COMPUTE MATRIX NORM :::::: 
DO 50 I 3 1, N 

DO liO J 3 K, N 

40 NORM = NORM ♦ DABS(H(I,J)) 



50 



K = I 
IF (I 

!!(![ 

CONTINUE 



.GE. LOW 
=* H(I.I) 

* 0.060 



. AND. 



LE. IGH) GO TO 50 



60 



70 



EN * IGH 
T = O.ODO 

:::::::::: SEARCH FOR NEXT EIGENVALUES :::::::::: 

IF (EN .LT. LOW) GO TO 1031 
ITS = 0 
NA = EN - 1 
ZNM 2 = N A - 1 

:::::::::: LOOK FOR SINGLE SMALL 3U3-CT AGONAL ELEMENT 

FOR L=5N STEP -1 UNTIL LOW PO — ::::::::: 
DO 30 LL 3 LOW, EN 
L * FN ♦ LOW - LL 
IF (L .2Q. LOW) GO TO 100 



I EnR 
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S = DABS (H (L-1.L-1 )) ♦ D ABS ( H (L, L) ) 

IF (S . EQ. O.ODO) S = NORM 

IP (DABS (H (L,L-1) ) .LE. MACHEP > S) GO — in * 

80 CONTINUE * v 100 

:::::::::: FORM SHIFT :::::::::: 

100 X - H (EN.EN) 

IF (L . EQ. EN) GO TO 270 

Y = H ( N A , N A ) 

W = H (EN .NA * H ( N A . E N) 

IF (L . EQ. NA) GO t6 280 

IF (ITS .EQ. 30} GO TO 1000 

IF (ITS .NE. 10 .AND. ITS . NE. 20} GO TO 

:::::::::: FORM EXCEPTIONAL SHIFT 

T = T ♦ X 

DO 120 I = LOW, EN 
120 H (I , I ) = H (1,1) - X 

S = DABS (H (EN.NA) ) ♦ DABS ( H (NA, EN M2) ) 

X = O.75D0 5 

Y = X 

W = - 0. 4 37 5 DO * S <* S 
130 ITS = ITS ♦ 1 

:::::::::: LOOK FOR TWO CON SHCUTIVE SMALL 

SOB-DIAGONAL ELEMENTS. 

FOR M = E N - 2 STEP -1 UNTIL L DO 

DO 140 MM = L, ENH2 

M = ENM2 ♦ L - MM 
ZZ = H ( H , M ) 

R = X - ZZ 

5 = Y - 2,Z 

P * (R * S - W) / H ( M ♦ 1 , M) ♦ H (3 , N + 1 ) 

Q = H (H + 1, M*1) - ZZ - R - S 
R * H (M + 2. H+ 1 ) 

S - D ABS (P ) ♦ DABS (Q) ♦ DA BS ( R) 

F = P / S 

Q * 0 / S 

R 3 R / S 

IF (M . EQ. L) GO TO 150 

IF (DABS (H (M ,M-1) ) 5 (DA BS (Q) ♦ DABS(R)> T P nRQC/n% 

X ft (DABS (H (3-1, M-1 ) ) ♦ DABS(!:Z) ♦ D A Us t f LiT J S JP) 

140 CONTINUE '‘’(^l, 11 *!)))) F,0 TO ISO 

150 MP2 = M ♦ 2 

DO 160 I - MP 2. EN 
H (1, 1-2) = 0.0D0 
IF (I .EQ. MP2) GO TO 150 
H ( I . I -3 ) = 0. 0 DO 

160 CONTINUE 

DOUBLE QR STEP INVOLVING ROWS \ T n fn 
COLUMNS M TO EN :::::::::: 1U tN AND 

DO 260 K = M, NA 

NOTLAS - K . NE. NA 
IF (K . EQ. M) GO TO 170 
? » U(E,K-1) 

Q = H (K ♦ 1 , K- 1 ) 

K = 0.0D0 

IF (NOTLAS) R = H(K*2,K- 1) 

X * DABS (?) ♦ DA BE (Q) ♦ DABS(P) 

IF (X .Ed. 0.0D0) dO TO 260 
P « P / X 

o ® o / j 

h = R / X 

170 S * DSIGN (DSQRT ( P^ P^R) , P) 

IF ( K . EQ. M) GO TO T 8 J 
HfK.K-n = -S * X 
GO TO 190 

1°0 IF (L .NE. M) H(K,K-1) • -H(K,K-1) 

190 P = P ♦ S 
X * ? / S 
Y 3 Q / S 
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c 

c 



200 

210 



ZZ = H / S 
Q = Q / ? 

R = R / P 

::::::: ROW MODIFICATION ::::: 

DO 210 J = K. EN 

P = H (K # J ) ♦ Q c H (K* 1, J) 

IF (.NOT. NOTLAS) GO TO 200 
R » H (K+2, J) 

= H (K* 2, J) 



P = P 



H(K+2,J) = H(K*2,J) - ? * ZZ 

H ( K+ 1, J) = H ( K+ 1, j} - P * Y 

h i 6 J ) * H (K# J) " P * X 



CO NT 
J = MIN0 (EN, K + 3) 

::::::: COLUMN MODIFICATION :::: 

DO 230 I = L, J 

P = X 9 H(I.K) ♦ Y * H (I, K ♦ 1) 
IF (.NOT. NOTLAS) GO TO 220 
F * P ^ “* 





H 


(I, K*2) 


= H (I # K+2) - 


P > E 


220 


H i 


I, K*1 


= H I, K*l| - 


? » Q 


230 


H 

CO NT ] 


\kV - 


H(I,K) - > 





260 CONTINUE 
GO TO 70 



270 



280 



WR(EN) =■ X 
WI(EN) = 0 
EN = NA 
GO TO 60 



ONE FOOT FOUND 

♦ T 

0D0 



: TWO ROOTS FOUND 
X) / 2.0D0 



P = ( Y 

Q a p t P ♦ V 

ZZ = DSQRT (DABS (Q) ) 

X » X ♦ T 

IF (Q . LT. 0. 0D0) GO TO 320 
:::::::::: REAL PAIR ::::::: 



WR ( N A 1 


1 = X 


WR(SN 


L = M 


IF (Z! 


Z . NE. 


wi (Na; 


1 = o 


WI( EN 1 


= 0 


GO TO 


330 


3 20 wr(na; 


i a i 


WR(EN 


= X 


WI (NA 


= z; 


WI (EN 




330 EN = 


ENM2 


GO TO 


60 


1000 IERR ; 


= EN 


1001 RETURN 


END 



♦ ZZ 

? ( o N *U 

. 0D0 



WR (EN) = X - W / ZZ 



COMPLEX PAIR 

♦ P 

♦ P 



SET ERROR -- NO CONVERGENCE TO 
EIGENVALUE AFTER 30 ITERATIONS 



AN 



LAST CARD OF HQR 



SUBROUTINE PS DC A L ( N 2 . NS . FA . X,NC # GW,IV # C # NO.HT # HU.H / 

1 F9GE,NG ,GAM, ACL , F, W 6 , * I , D 1 ,D2,JCF # RSS, Q, & , do , CC , ITU , 

2 IPS D , I NOR M) 

PSDCAL COMPUTES THF PSD OP OUTPUTS OR CONTROLS OF 
A CCNT ROLLED STSI EM 

IYU» 1 OUTPUT PSD 

* 2 CONTROL PSD 

I P SD * 1 PSD 

*2 PSD AND TF RESIDUES 



90 



noon o n oooon 



INORM* 



NG NORMALIZED BY ITH PROCESS NOISE 
NG+NO NORMALIZED E Y ITH M 2 A S NOISE 



1,2.... 
NG+1 ,. . . 



DOOBLE PRECISION F A . X , GW , 3 V , C , H Y , H, FB GE , G A M , ACL, ? , V R , WI , D 1 , D2 , RES 
1 8S,CC,Q.R ,PSD, W,DNORM,DN1 , &MAX , FLOG, EMOD , DW , ST, 0 M , R E, A I , HU , D W 1 
COMPLEX^ 16 CD,ZN,ZZ 







HI i» S0 6Hili5i 

o ) # d rv { W J | 



W{ 30) , 3B(N2) L Z 
I Nr EG JCF(N2) 
DATA DW 1/1. DO ,2 






DO, 5- DO, 10. DO/ 



IF ( I Y U . EQ . 0) I Y U= 1 
IFJINORH .EQ. 0) INORM = 1 
IPT = 0 

IF ( I P SD .GT. 1) IPT = 1 



II = INORM - NG 
IFjlX .GT. 0) WRITE(6 ,0000) 
FORMAT (/ ' SUBSEQUENT PSD IS 
IF ( IX . LE. 0) WRITE [6 ,00 10) 
0010 FOR MAT (/ ' SUBSEQUENT PSD IS 
NSQ = N2^N2 



IX 

NORMALIZED BY MEAS NO 
INORM 

NORMALIZED BY PROCESS 



M3) 

NOISE NO. 



13) 



COMPUTE EIGENSYSTEM OP CONTROLLED SYSTEM 



:::::::::: FORM FA 

DO 10 1*1, NS 
DO 10 J = 1 , NS 
FA(I,J) = ACL (I , J) 

10 F A ( Ns ♦! , J) = 0. DO 
DO 20 1=1, NS 
DO 20 J = 1 , NS 
ST = 0. DO 
DO 15 K= 1 , NO 

15 ST = ST ♦ FBGE (I ,K) c*H (K, J) 

FA(I,NS*J) = -ST 
20 FA ( NS ♦ I , NS ♦ J) * F(I,J) * ST 

CALL R A f FNT (N 2, N 2 , N 2 , 9, P A, 4 , • (9(1X,1PD13.6)) ') 
CC •OCtin*itr.cm.r * * DEBUG ABOVE 

CALL BALANC (N2, N2, PA, LOW f TH IGH, D 1) 

CALL ORTHPS (N2, N2, LOW ,IHIGH , FA, D2) 

CALL OPTRA N JN2. N2 . LOW , IH IGH . FA, D2 , X) 

CALL HOF 2 ( N^ , n5 , LOW .1 HIGH. ? A, WR , WI, X, I ERR) 

IF(IEF$ .NE. 0) GO TO 1006 

CALL BALSA K (N2 f N2 . LOW f I H IGH f Dl f N2.X) 

CALL RAFPNT ( N 2 , N2,N2,9,X,4, • (9 ( 1 X, 1PD 1 3 . 6) ) ■ ) 
COf»ntt£r>c<»rr*-vf; DEBUG ABOVE 
100 CONTINUE 

C :::::::::: DETERMINE MODAL MATRICES 

IF ( IYU . EQ. 1} GO TO 130 
C MSUBU 

DO 110 1=1, nc 
DO 110 J=1 , N2 
ST = 0. DO 
DO 105 K = 1 , NS 

105 ST = ST - C (I ,K) ®X (K, J) 

no hu(i.j) = si 

GO t6 150 



C HSU3Y 

130 DO 14 0 1=1 , NO 
DO 140 J=1 , N2 
ST = 0. DO 
DO 135 K = 1 , NS 

135 ST = ST ♦ H f I , K ) 0 X ( K, J) - H (I,K)*X(KS+K,J) 

140 H Y ( I , J) = ST 

CALL PAPRNT (NO, NO, N2, 9, H Y, 4 , • (9 ( 1 X , 1 ? D 1 3 . 6 ) ) • ) 
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DEBOG ABOVE 

150 CALL BINV(NSQ.X,N2,ST £ D1 ,D2) 

CALL R A ? RNT (N2,N2,N2,§,X,4, »(9(1X,1PD13.6)) ') 

DEBOG ABOVE 

C :::::::: :: GS UBM 

DO 160 1=1, N2 
DO 160 J = 1 , KG 
ST = 0. 0D0 
DO 155 K=1 , NS 

155 ST = ST - X(I,NS«-K) "’G AM (K, J) 

160 G V ( I , J) = ST 

CALL RAP RNT (N2, N2, NG. 9, GW, 4 , • (9 ( 1 X , 1 P D 1 3 . 6 ) ) •) 

DEBUG ABOVE 

C :::::::::: use SELECTED KDR M aliz ATION 

200 IFflHORM . LE. NG) DNORM = 1 . D0/Q (I NOR M , I NO F 5) 

IF(INORtt .GT. NG) DNO RM = 1 . DO/R (INOR H - NG , I KORN- NG) 
8ANDtf IDTH OF CONTROLLED SYS 



210 



DETERMINE 

EMAX = 0 . DO 
DO 210 1=1 , N2 

EMOD = DABS (WR (I) *?2 ♦ ¥I(I)Ort2) 
IF(EMOP .GT. EMAX) EM AX = EMOD 
CONTINUE 



EMOD = DSORT(EMAX) 
3=2* EMOD 



1 



EMO D= _ 

C :::::::::: ROUND UP TO NEAREST 2,4,5,8,10 

SLOG = DLOGIOfEHOD) 

I F ( ELOG .LT. 0. DO) I?OW = - I DI N T (D A B S ( ELOG) ♦ 1) 

I F ( E L OG .GE. 0- DO) IPOW = IDINT(ELO^) 

EMAX = EMOD^K)** 1 f-IPOW) 

IFfEMAX .GT. 2. DO) EMOD = 2 . DO 
I F ( EM AX .GT. 4. DO) EMOD = 4 . DO 

IFfEMAX .GT. 5. DO) EMOD = 5 . DO 

IFfEMAX .GT. 8. DOf EMOD = 9 . DO 
IFfEMAX .GE. 10. DO) EMOD = 10. DO 
EMAX = EMOD*10*OIPO¥ 

DM = EMAX/20.D0 

C :::::::::: ADD 10 POINTS 3 DECADES UP 

I P f EMOD .LT. 5.0) GO TO 212 
EMAX = 1.0D1 
IK = 3 
GO TO 216 
212 EMAX = 5. DO 
IK = 2 

216 CONTINUE 

C :::::::::: STORE 30 FREQUENCIES 

- DO 220 1=1,20 
220 W^I^ = DV* (1-1) 



18 1=1 , 3 
IP = 20 ♦ 3 ^ ( I - 1 ) 

DO 218 J= 1 , 3 

IX = MOD (IK + J-1, 3) 
JJ = 0 

IF (IK .EQ. 2 .AND. 



♦ 1 



.GE. 2) JJ= 1 



W(IP*J) = DW1 (IX) ° 1 0 ^ ( I POW ♦ I - 1 ♦ J J ♦ I K - 2 ) 

218 CONTINUE 

IX = MOD(IK,3) ♦ 1 
U (3 0) = uWI (IX) *MO**(IPOW*3 *IK-2) 

LARGE LOOP THRO OUTPUTS 

1) NL= NO 

2) NL * NC 



250 



IFj 


; I Y U . FQ 


I F | 


ITU .EO 


DO 


400 L* i 




DO 250 




PSD(I) 



* 0 . DO 

LOOP THRO PROCESS NOISE 
DO 3 JO 1 = 1 t NG 
dn6rm*<; 



8020 



DN 1 = DNOR M fc Q (I . I) 

IFfJTU .EQ. 1 .AND. I F i .ED. 1) WPIT5 (6 , 80 20) I.L 
FOR M AT ( / 1 TRANSFER FUNCTION FROM PROCESS NOISE ‘,12, 
1, • MEASUREMENT *12) 

!P(IYO .ZQ. 2 .AND. XPT .ZD. 1) WRITE (6, 8030) I f L 
“ NCT ION FROM PROCESS NOISE r ,I2, 



8030 FOR M AT (/* TPANSFEP FUNCT 
1* CONTROL • ,12) 



TO * 

TO* , 
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254 

256 



IF (IYU.2Q. 1) CALL RES ID (I,L,N2, JIT? 
RES,B3,CC.IPT) 

IF (IYU.SQ. 2) CALL RESID (I,L,N2, J’C? 

RES. 39, CC , xPT) 

DO 260 6=1,20 

22 = DCM PL X (0. DO, 0. 00) 

Or. = W (K) 

DO 260 11*1. N2 

IF (MI (II) ) 2 60 , 25U ,256 

2D = DCMPLX(-WR (II) .OM-WI (IT) s 
22 = RES (II) /2D ♦ ZZ } 

GO TO 260 
RE = W R (II 



■«g # gb,ml, hy,we,wi, 

, G W , NL, HU, MR, MI, 



= W R (II) 

= MljllJ 

4U = DCMPLXfEE*^ ♦ AI^*2 - : - • 0 -> 

2 N = DCrtPLX FES (II ♦!) > AI-RE? iSp^R^/Tri 2nL 

22 = 2Z ♦ 2I./2D v - • ) * «E, RES (II) ^OM) 



A I 
ZD 



,ZZ) ) 



260 CONTINUE 

280 PSD ( K) = PSD (K ) ♦ ON 1* (2 Z*DCC N - s 

300 CONTINUE 
C :::::::::: GSUBV 

DO 320 1=1 , N2 
DO 320 J=1 , NO 
ST = 0. DO 
DO 315 K = 1,NS 

315 ST = ST ♦ X (I,K) $F5GE (K, J) ♦ X(I,NS*K) >»>c P7/r 
320 G V ( I , J) = ST ' bG ~ 

CALL FAPRNT (N2. N2, NO. 9.GV/4 ,♦ (9 (IX, 1?*.M ; n 
DEBUG ABOVE ^* b )) ) 

C :::::::::: LOOP THRU r.EAS NOISE 

DO 390 1 = 1 , NO 

D N 1 = DNORK^R (I .1) 

I F ( I V U .EQ. 1 -AND. IPT .EQ. 1) MRITF.> o n/1 m r t 
8040 FOR MAT (/ ' TRANSFER FUNCTION FROM ME AS , 1 1 5r 

1 * TO MEASUREMENT * , 1 2) ‘"“NT , 12 , 

IF ( I YU .EQ. 2 .AND. IPT . EQ . 1) WRITE r 6 fl/Km T r 
8050 FOF M A T (/• TRANSFER FUNCTION FROM MF VS $ R ^ , 1 t k 

1 • TO CONTROL • 1 2) - NT ,12, 

IF (IYU. EQ. 1) CALL PE SI D (I , L # N2 , JCF , S WT „ v UD .. T 

1 BD.CC,I?T) * "V,NL, HY, MR, fc I , RES, 



IF(tYU.'E0.2)CALL SE SID ( I , L, N2 , JCF , S J , v ut H1I wr u t Br< . 

gn cc IPT) Wi.,nU, ■ R , ■ I , R ES , 

DO 580'k = 1,30 



354 

356 

360 



378 

380 

390 



9000 



22 = DCM.faX (0. DO, 0. DO) 

360 ( li=1 ,N2 

IF (MI (II) ) 360. 354 .35 6 

2D = DCMPlX (-W& (Ilf .QM-WI (II) ) 
22 * 22 ♦ RES (I J) /Zu 



GO TO 360 
PE =W R (II) 
a i = w: (ii) 

2D = DC MPLX ( R E* *•' 2 ♦ A I*** ^2 -OM>>? 



2N = DCHPLX (RES (il^lj'^AI-RES’iirflpS-PS^f!^) . 
Z 2 = Z2 ♦ ZN/ZD J t , R-S (H ) ®OM) 

I 



psd ir 



CONTINUE 
IFfITU .EQ. 2 .OR. 



?SD(K) 
PbD(K) = PSD(K) 
COVTINUF 
CONTINUE 



.ME. L) GO TO 378 
♦ DN 1 

♦ DN1* (ZZ^DCONJG (ZZ) j 



I P ( J Y U .EQ. 1) -RITE(6,903Q) L 

IFjlTU .EQ. 2) WRITE? 6, 9013 ) L 
FOr. M AT (/* PSD OF OUTPUT', 13,' 

" "^O M ‘ * ** 



1 , ' NORMALIZED PSD) '/) 

9010 FOR M AT ( / * PSD OF CONTF.i 
1 , ' NORMALIZED PSD) '/) 

-I p L. 



OL' , I : 



WRilE (6,^020) (W(l{ ,P$D(I) ,1*1.30) 

90 2 0 FOR MAT (4 (1 X / (' , ill . 4 , ' , • , 2 1 1 . 4 , • j ')> 
400 CONTINUE 



FORCED [1 f ALL NOISE-(PAD FREQ,' 
FORCFD h Y all KOI5E- (RAD Fa EQ , ' 



R ET U ? N 
1000 CONTINUE 
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noon 



C 



9000 



CALL EREXIT (N2 , PA , I ER R) 

RETURN 

END 



SUBROUTINE EREXIT (N, A, IERR) 

EREXIT RETURNS THE NUMBER OF THE El GEN VAL 
FAILS, THEN STOPS THE PROGRAM. 

INTEGER IERR 
DOUBLE PRECISION A 
DIMENSION A (N , N) 

WRITE (6 # 9000) IERR 

FORMAT*' FAILURE IN HQR2 ON EIGENVALUE NO. 
CALL RAPENT (N,N ,N,9, A, 4 , • (9 ( 1 X, 1 PD1 3. 6) ) • ) 
STOP 
END 



Z WHERE HQR2 



,I3| 
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ccccccccccccccccccccccccccccccccc 

c 

SENSITIVITY COVARIA 



c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



cccccccccr cccccccccccccccc 

NCE PRO 3 HA a 

SENSITIVITY 
PTHFrtf , IMPLEMENTATION 
F THE KALIAN FILTER. THE 



FV+VTDFT*303T*KvRK$T 
F :> -K^H) T+UDFT-GQGT T 



THIS PROGRAM IS USED TO SOL 
EQUATIONS WHEN THERE IS AN 
OF DYNAMICS IN THE DESIGN 3 
EQUATIONS BECOME 

PDO T= (F* -K k H) F*P(F*-K**H) T+D 

V DO T = FV* V (F ;V ~ H‘ 

UDOT= FU*Ur T *G QGT 



THE PRINCIPAL PROGRAM INPUT 
LLECT ION OF SYSTEM AND FILT 

PO THE INITIAL COVARIANCE 

F THE TRUTH MODEL DYNAMI 

THE FILTER MODEL DYNA1 
H THE TRUTH MODEL MEASUR 
L IS THE MEASUREMENT V 
GQGT THE INPUT NOISE COVARI 

R THE MEASUREMENT NOISE 

K<* FILTER GAIN (NX L) 



fo A 2F-?V E F0 - LOWING CO- 
ER lAifliC; S 



MATRIX (NX s 
CS MATR^ — 



CS MATRIX f n1( N) 

ICS MATRIX (NXN) 

EMENT MATRl'x <LXN) vhe 
ECTOR DIMENSION ' 

ANCE MATRI X (NX) 

COVARIANCE MATRIX (LXL) 

CCCCCC CCCC CCCCCCCCCCCCCCCC 

rHE Itt5L LIBRARY 
cNTER OF THE NAVAL 



RE 



CCC CCCC CCCC CCCCCCCCCCCCCCCC CCCCCC 

c 



c 

c 

c 

c 

c 

c 



THIS PROGRAM HAS BEEN DEVEL 
AVAILABLE IN THE COMPUTER C 
POSTGRADUATE SCHOOL 



IMPLICIT REAL38 (A-H,0-Z) 
COM “ - ~ “ 

COMMON’/KTfc/N, NS,HPD 
DIMENSION PFULL (7, 7) , PSQR (7 



OMMON F(7,7) ,PSj7,7) ,GQGT ( 7) ,AK (7. 2l ,p (2 2\ 
KRKT (7 ,1\ r Dr (7 . 7) , FT (7,7) , FsfiK Hi (Mf.dF* {l\ 
c*FSMKH (7,7) ,H (2. 7 M ' 

nn v / t't: / c 'mn 



7 ) , 



DIMENSION DDT (7) 



DIMENSION U (2 
T, V AR ( 1 05) , DR v (1 
[MANSION TMP 1 



V (7,7) ,P (29) 



c 

c 

c 

c 

c 

c 

c 

c 

c 



c 

c 

c 

c 

c 

c 



UO°J2«. , VD (7 ,7) , 

DIM iN SI 5ft' T M £* 1 (7^ )j T M £ 2 (7,7) . f MP 5 P (7 ^ ) * 
EQUIVALENCE ( U (1 f , V XR ( 1 ) ) . ( V (\ 1 ) ') A ft r Lq, ) , (T ( 1 \ , 

$var |78j|, ( uuh),b6v(i,),(H(i^),6RV(i 9) | , ( r d ( i J ; 

N=0 RD ER OF THE SYSTEM MODEL 
N P= NUMBER 0? POINTS 

N PD =C 0N7 PO L OF INITIAL DIAGNOSTIC OUTPUT 

DT 3 TIME INTERVAL 

EXTFRNAL PUN 
CALL UGETIO (3,5,6) 



THE FOLLOWING SECTION READS 
MATRICES, F,r», GQGT, K-H AND 



THE SPEC I PI SD INPUT 




SS= S* (N» 1) /2 
N 1 * S S ♦ ^ 2 ♦ 1 

2 * NS^ K** 2 



NV 



99 FC?MAT(BF10.5) 
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noooooooonn non nooooo noon 



noon 






1 

c 


DO 1 1=1. N 

READ (5,9§) (P (I . J) , J= 1,N) 
CALL OSWFM ('F' , 1 , F, 7, N, N, 1) 


2 


DO 2 1=1. N 

READ (5, 9 9) (PS (I.J) # J = 1,N) 

CALL USWFM ('FS* ,2,?S, 7.N.N, 1) 

R EA D (5,99) (GQGT(I) ,1=1,8) 

CALL OSWFV (•GQGT* ,4,GQGT,N, 1, 1) 


C 




3 


DO 3 1=1. N 

R EA D ( 5 , 9 91 (AK(I,J) ,J = 1,2) 
CALL US W Frt ( f K * , 1 ,Ak,7,N, 2 , 1) 


C 




4 


DO 4 1=1.2 

R EA D ( 5 , 9 9) (H (I.J) ,J=1,N) 
CALL U5W FM ( ' H ' , l , H, 2, 2, N , 1) 


C 




5 


DO 5 1=1.2 

READ (5, 9 9) (R (I.J),J=1,2) 
CALL USWFtl ( 'R' , 1, R, 2, 2,2, 1) 


C 




6 


DO 6 1=1,105 
VAR (I) =0. 

DO 7 1=1, N 
DO 7 J=1,K 


C 

7 

C 

C 

c 

c 


DF ( I , J) =FS (I.J) -P (I.J) 

CALL USWFrtj'OEl F « , b, DP, 7 . N , N , 1 ) 

CALL V5ULFF (AK,R,N.2, 2,7.2, T5P1 .7,rER) 

CALL V EULP P (T HP 1 . A K , N . 2, N, 7 , 7 , A$ RK?, 7 , I ER) 

CALL USWFrt ( , KRKT',4,AKP.KT,7,N,N, 1) 

CALCULATE THE DIFFERENCE BETWEEN THE DYNAMICS 
IKP LE H ENTED IN THE FILTER AND THE PLANT, DF = F^>-F 


20 


DO 20 1=1, N 
DO 20 J = 1 , N 
DFT (I.J) =6 F (I.J) 

FT (T , J) = F (I , J ) 

CALL ViRANXjDFT , N , N , 7 ) 

CALL US m FH ( * D EL FT • 6 , DFT, 7 , N , N , 1 ) 

CALL VTF.ANX (FT.N, N,7) 

CALL USWFH(’FT*.2.FT, 7,N,N, 1) 

CALL VnULFF (AK,H,N,2, N,7,2, THP1 ,7,IER) 


C 


DO 21 1 = 1,7 


21 


DO 21 J = 1 . 7 

FSHKH (I.J) =FS (I. J) -TMP1 (I, J) 

FS.nKHT(I.J) =PS5KHfI,j[ 

CALL U 5 V P*1 ('FS-KH',5, FSMKH, 7,N,N, 1) 

CALL VTRANi (PSr.KHT, N. N,71 

CALL USWFfl ( MFS-KH) T f , 6, FSH KHT, 7 , N, N, 1) 

T=0 . 

TOL=1 .D-5 
I ND = 1 
L=0 

I F ( N . EQ. 7) GO TO 11 


C 


DO 31 1=1, NS 
L=L ♦ 1 


3 1 
C 


VAR (L) =U (I) 

DO 32 1=1, N 
DO 32 J = 1 , N 
L=L ♦ 1 


32 

C 


VAR (L) = V (I, J) 

DO 33 1=1,5 
L = L ♦ 1 


33 
1 1 


VAR (L) =P f 11 
DO 10 F= i , N P 
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on no n o non on non noon on 



TEN D= FINAL TINE 
TEN D=K»DT 



OVER K SUBROUTINE FINDS THE SOLUTION OF THE SYSTEM OF 
DIFFERENTIAL EQUATIONS 

CALL DVERK fNV.FUK, T, VAR, TEND, 70L,IND,C, 105,WK,IER) 
IF( IND.LE. 0.0 IER. NE .0) STO P 
CALL VCVTSP (VAR ( N 1 ) ,N, PFULL ,7) 

CALCULATE AND PRINT THE RrtS ESTINATE ERRORS 



30 

90 



DO 30 I = 1 , N 



REAF = PFULt (I - I) 

PFULL (1,1) = DABS (REAP) 

PSQ R ( f ) = DSQRI (PFULL (I ,1) ) 

WRI TE (6 f 90 ) T, JPSQR (I) ,1= 1 f N ) 
FORMAT ( ( 0T = \F10. 5, • f>SR= f ,7 



G 15 .7) 



IF DESIRED PRINT THE COVARIANCE MATRICES, P,U AND V 
CALL USWSK 
CALL OSWFN 
CALL USWSK 
10 CONTINUE 
STOP 
END 

SUBROUTINE VTR AN X (A , N . NC . I A ) 

IMPLICIT REALMS (A-H,0-Z) 

DIMENSION A (1 A , I A) , B ( 7 , 7 j 



( , 0 , # 1 r U|H # 2) 4 „ 

(' V* f 1, vAa ( NS* 1) , 7,N, N,2| 
•P* r 1,VAR N1) ,N, 2) 



DO 1 1=1, N 
DO 1 J= 1 , N 
1 E(I,J)*A (J,I) 

DO 2 1*1, N 
DO 2 J= 1 , N 
2 AjI,J|*B (I,J) 

RlTURN 

END 

SUBROUTINE FUN(NV,T,V AR , DR?) 



FCN SUBROUTINE IS USED FOR EVALUATING F U NCT IONS ( I N PU T) 



C 



C 



C 



IMPLICIT REALMS 
COMMON F(7,7) ,PSj7,7( ,GQOT(7) ,AK (7.21 , P (2,2), 
«?AKRKT (7,7) , Dr (1 L 7) , FT (7 , 7) , FSKK H T ( 7 , 7 f , OFT (7 , 7) , 
n F E M K H (7.7i , H ( 2, 7) 

COMMON/KTR/N. NS , NPD 

DIMENSION U(2 8) i V(7,7),P(28).UDf28).VDf7,7) , 

*VAH (105) , DRV (105) ,C (2M ,WK ( 105, , PD ( 28 ) 

DIM EN Slcfr T M P 1 ( 7 , ^ ) ,IKW(7;7) (7; 7| 



(A-K, O-Z) 



1 

2 

3 

99 



L=0 

DO 1 1*1, NS 

L=L ♦ 1 

U (I ) = VAR (L) 



DO 2 J= 1 , N 
L*L ♦ 1 

V (I , J) =VAR (L) 

DO 3 1*1, NS 
L*L ♦ 1 

P (I) *VAP fL) 

IF (T. EO. i) KT=NPD 
KT- KT * * 

IF {KT. GF. 5) GO TO 15 
WFiI?E(6.99) T 
FORMATf'OT* 1 , D25. 15) 
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c 



c 



c 



c 



c 



c 



c 



15 



CALL OSWSH N, 2) 

CALL OSWFfl (' V* , 1 , V, 7, N, N,2) 
CALL USWSK f • P • 1 -V- N. 2) 

CALL VMULFS (F,U,N,N, 7 r TM Pi r 7) 
IF ( KT. LT . 5 ) CALL USWFH (' FU* , 2 , 
CALL V M U LS r (U r N,FT,N,7,T?iP2 . 7 ) 
IF ( KT . LT . 5 ) CALL USWF 3 ( 1 OFT » , 3 



r«P1 ,7, N, N, 2) 
f T3P2 r 7 , K , N r 2) 



OO 5 1=1 ,N 
00 U J=1,N 

4 TMP 1 

5 T-P 1 

CALL " VCVTF S (T flP 1 r N 
IF ( KT . LT . 5 ) CALL " 
- - J - ? F (P 



(I,jf=TKP1 (I,J) +TMP2 (I. J) 
(1,1) = T.1P 1 jlr I) *G QGT (if 



u£w§M fl 00 0 T * .4 .00, N, 2) 

«: yr r ,N. N,N,7.7 r mPl, 7,l£R) 

-LT.5) CALL USWFrt(* FV« 2 .T 3P 1 . 7 . N , N . 2) 
VrtULFF(V.FSHKHT,N,N / N,7.7,THP2,/.IE&) 

. LT. 5 ) CALL USWFfl (• V* ( FS-KH) T* , 10, I3P2,7,N, N, 
VMULSF (U.N ,DPT,N,7,TriP3,7) 



CALL VMULFr (F , V 
IF ( KT * “ " ‘ ~ r * 

CALL 
IF ( KT 

CALL VMULSP (U.N-DFT-N.7. TMP3.7) 

IF ( KT . LT- 5 ) CALL US WF ft ( » U* 0 FT • , 5, T3P3 , 7 , jf , N , 2) 

DO 7 1=1 ,N 
00 6 J=1,N 

VD(I, J) = TMP1 (I- J) ♦ TKP2(I,J) ♦TKP3 (I f J) 



2 ) 



) ( I , J) =TMP1 (I, J) 

VD(I, if =VD (I -if -GQGT(I) 

IF ( KT . LT. 5 ) CALL U S WF R ( • 7 DO T • , 4 , V D f 7 , M , N , 2) 

CALL VMULr S (FS tt KH - P - N , N , 7 - T3P1 -7) 

IF ( KT - LT . 5f CALL U$ Wp« (• (FS -KH) P» 8 f TrtP 1 , 7 , N, N, 2) 
CALL VHULSF (P r N r FS3KHT,N-7, T J1 P2 , 7 ) 

IF f KT - LT . 5 ) CALL U S WF H ( 1 P jf P S- KH) T • 9 - Til P 2 , 7 , N , N , 2) 
CALL VJ1ULFF (OF- V, N. N, N,7 f 7, T3P3 , 7, I eS ) 

IF ( KT . LT • 5 ) CALL US Wf K (• DF* V • , 4 ,T3P3, 7 , N , N , 2) 



DO 8 1=1 ,N 
00 8 J=1 ,N 

8 T3P i 
CALL 
IF (KT 

DO 10 1=1. M 
00 9 J= 1 - N 

9 TWP3 (I, jf = THP 1 (I, J) +TKP3 (I, J) ♦AKRKT (I , J) 

10 TaP3JI / I =TttP3 (I, if ♦ 



0 8 J=1.N 

HP 1 (I, jf = T3P 1 (I, J) *TKP2 (I, J) *TMP3 (I. J) 

AL L VrULF M (V , DF , N - N, N, 7 , 7 f TJ1P3 , 7 , I Eft ) 

F( KT. LT-5) CALL US WF « ( • VT* DF • , S , TMP3 , 7 , N , N , 2) 



IF(KT.tT.5f CALL r U5WF 



CALL VCVTP^ (TrtP3, N. 7, Pti) 
IFjKT-LT-5) CALL USWFV( V 






T* ,4 ,TMP3,7,N, N,2) 
PDDT(Sia) 9 ,9, PD,N, 1 r 2) 



00 11 1= 1 , N S 
L=L ♦ 1 

11 DRV ( L) =U 0 ( I) 

OO 12 1=1, N 
OO 12 J=1,N 
L-L ♦ 1 

12 DRV (L) =VD(I,J) 

OO 13 1 = 1, NS 
L = L ♦ 1 

13 DP V (L) =P D ( I ) 

IF(KT-LT.b) CALL OSWF V ( • OR V • , 3 , DP V, NV , 1 , 2) 

RETURN 

END 
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ccccccccccccccccccccccccccccccccc 

c 

SENSITIVITY COVARIA 



THIS PROGRAM IS USED TO SOL 
EQUATIONS WHEN THERE IS AN 
OF DYNAMICS IN THE DESIGN 0 
EQUATIONS BECOME 

PD0?= (F*-K*H) P+?(P*-K^H) T*D 
VDOT=rV+V { F n - K* H) T+UDFT-GQG 
UDO T= FU+UPT+GQGT 

THE PRINCIPAL PROGRAM INPUT 
LLECTION OP SYSTEM AND FILT 



CCCCCCCCCCCCCCCCCCCCCCCCCC 
NCE PROGRAM 

VE THE ER30F. SENSITIVITY 
INCORRECT IMPLEMENTATION 
F THE KALMAN FILTER. THE 



FV + VTDPT + GQGT+K* - RK*T 
T 



S AR E THE FOLLOWING 
ER MATRICES 



:o- 



PO THE INITIAL COVARIANCE 

F THE TRUTH MODEL DY N AMI 

THE FILTER MODEL DYNAM 
H THE TRUTH MODEL MEASUR 

L IS THE MEASUREMENT V 
GQGT THE INPUT NOISE COVARI 

R THE MEASUREMENT NOISE 

K* FILTER GAIN (NXL) 



MATRIX (NXN) 

CS MATRIX ( N X N ) 

ICS MATRIX(NXN) 

EMENT MATR IX (LXN) , WHERE 
ECTOR DIMENSION 
ANCE MATRIX (NX) 
COVARIANCE MATRIX (LXL) 



CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

c 



c 

c 

c 

c 



THIS PROGRAM 
AVAILABLE IN 
POSTGRADUATE 



HAS BEEN DEV! 
THE COMPUTER 
SCHOOL 



OPED USING THE IMSL LIBRARY 
ENTER OP THE NAVAL 



C 

C 

C 

c 

c 

c 

c 

c 

c 



IMPLICIT REALMS (\-H,0-Z) 
COMMON F(8,8) ,?S “ 
rt AKR KT (3 , 8) , DF (8. 

*FSM KH (8,8) ,H(3 f 8 
COMMON/KTR/N, NS ,NPD 
DIMENSION PFULL (8,8) , PSQR(8 
DIMENSION DDT^8) 






DIM ENSIGN 



U (36). V (8,3) . P (36 
C ‘VAP(135) - DRV(13o) ,C(2uf,*K( 
~ &) # T M P 2 (8, 



II * V n 1 1 

DIMENSION TMP1 
EQUIV ALENCE 
eVAR (101)), (U 
ODRV (101 



PI (8.8 . T M 
(Ojl) ,VX f ( 
d (t) ,6av( 1 



D ) , ( 
) ) . (v 



) 

9) t r MP3 (8. 9) 

V(l, 1) i 'AR 07) ) , (P(1) 

D 1, 1) , DR V (37) j , (PD ( 1 ) , 



N=0 RD ER OF THE SYSTEM MODEL 
N P* NU MB EP OF POINTS 

N PD = CON T RO L OF INITIAL DIAGNOSTIC OUTPUT 
DT= T I ME INTERVAL 



C 

C 

C 

c 

c 

c 



c 



EXTERNAL FUN 
CALL UGETIO (3,5,6) 



THE FOLLOWING SECTION READS THE SPECIFIED INPUT 
MATRICES, F,F*, GQGT, H AND R 
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R EA D ( 5 , 9 8) N,NP,NPD, DT 
FOP M A T { 3 15 . rIO. 5) 

WRITE (6. 97) N,NP,NPD,DT 
FORMAT ( l 1* ,315, U,G 16. 10) 
NS^N* (N4 1)/2 
N NS ♦ + 1 



N V 3 2^ NS ♦ 2 

FOR MAT(8F10.5) 
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nnnnnnnonnonnnnnnnnnnonn 



nooo 



1 



DO 1 1*1 * 



READ (5, ?4) (F (I,J) , J* 1,N) 

CALL OSVFM (•?' ,1,?,8, N,N,1) 

DO 2 I* 1-3 

READ (5,94) (?S (I-J) , J = 1, N) 

CALL USWFM (’PS* ,5,PS,3-N,M, 1) 
READ (5,9 9) (GQGT(I) f I = i J) 

CALL OS W FV (•G^GT , ,4,GQOr,M, 1,1) 

DO 3 1*1-3 

R EA D (5,94) JAK(I,J) ,J=1,3) 

CALL USWF&(*K',1,Ak,8,N,3,1) 

DO U 1*1-3 

READ (5,94) (H (I -J) , J* 1,N) 

CALL GSWPM ( • H* , 1 ,H, J, 3,3, 1) 



DO 5 1*1,3 



READ (5,94) (R (I - J) , J= 1,3 
CALL USW FN ( * R • ,i,k,3,3,3. 



1> 



DO 5 t=1 , 1 36 
VAR (I) *0. 

DO 7 1=1, N 
DO 7 J=1 ,N 



DP ( I , J) = FS (I. J) -F (I-J) 

CALL USWFM('DEl ? *, 5, DF- 3. N , N. 1 ) 

CALL VrtULP r (AK, R, 3, 3, 3-d- 3. TMt>1 , 8, I ER) 

CALL VKULFPjTMP 1 - AK,N,3,N,d ,3,AKPKT,3 , IER) 
CALL US W FM ( • X 8KT * ,4,AXRK7-3 -N.M. 11 



:,3 ,n,n, 1) 



20 



CALCULATE THE DIFFERENCE BETWEEN THE DYNAMICS 
IMPLEMENTED IN THE FILTER AND THE PLANT, DF=F , " t - 

DO 20 1=1, N 
DO 20 J= 1 - N 
DFT { I - J ) =6? (I- J) 



U £ L i.U -u; i i. , 

FT(I,J)=F(I,jf 
CALL VTRANX(DF~ 



CALL (JS W F M ( 'DEL * F t * '6 , DPT, 3 ,N,N, 1) 
CALL VTRANJC (FT-N-N-8) 



21 



31 



32 



33 

1 1 



FT,N N,8) 

EL FT • - 6 , 

v. al l finn.iA.(i:T-N,N-d) 

CALL OSWFM ( , FT f . 5 . PT, 8,N,M, 1) 

CALL VHULFF (AK,H, N,3, N,8,3, ThPI ,8,IER) 

DO 21 1=1,8 
DO 21 J = 1, 8 

FSMKH (I, J) =FS (I,J) -TKP1 (I, J) 

FSMKHTJI.J) =FSMKH (I,J)_ 

CALL USWFM (’FS-KH* , 5, PSMKH, 8,N, N, 1) 

CALL VTR AN 2 (FSMKHT. N. N.8) 

CALL OSWFM (' (FS-KH)T' , 5 , FS 3 KHT, 8 , N , N , 1) 
T=0 • 

TOL = 1 . D- 5 
IND= 1 
L=0 

IF ( N • EQ.8) GO TO 11 

DO 31 1=1, NS 
L=L ♦ 1 

VAR (L) =U (I) 

DO 32 1=1, N 
DO 32 J= 1 , N 
L=L ♦ 1 

VAR (L ) = V (I , J) 

DO 33 1=1, N 
L=L + 1 



VAR ( L ) =P Cl) 
DO TO K=T,NE 



100 



noon o o onnno non noon nn 



TEN D=FI N AL TIME 
TEN D= KODT 



DVEF.K SUBROUTINE FINDS THE SOLUTION OF THE SYSTEM OF 
DIFFERENTIAL EQUATIONS 



CALL DVERK ( NV .FUN, T, VAR, TEN D,TOL,IND, C , 1 36 , WK, IEP) 
IF( IND. LE. O.OR.IER. NE.O) STD P 
CALL VCVTSF (VAR (N1) ,N ,PPULL ,8) 



CALCULATE AND PRINT THE RHS ESTIMATE ERRORS 



30 

90 



DO 30 1=1, N 
REA P= PFULL (I. I) 

PFULL (1,1) =DAB5 (REAP) 
PSQMI) =DSQRT (PFULL (I ,1) ) 
WRITE (6. 90) T, jPSQR (I) ' * 

FORMAT ( ; 0T= f , F10.5, ' 






G14. 7) 



IF DESIRED FRINT THE COVARIANCE MATRICES, P,U AND V 
CALL USWSM ( * U 1 
CALL USWFM ( * V • 

CALL USWSM ' P* 

10 CONTINUE 
STOP 
END 

SUBROUTINE VT R A NX ( A , U , NC, I A ) 

IMPLICIT REAL$8 (A-H,0-2J 
DIMENSION A (I A , I A) , B ( 8 , 8) 



, 1,0. N, 3) 

, 1 , V A R ( NS*' 1 ) , 8,N,N,3) 
, 1,VAR N1) ,h, 3) 



1 



2 



DO 1 1=1, N 

DO 1 0=1, N 
3 (I,J) = A (J,I) 

DO 2 1=1, N 
DO 2 J= 1 , N 



END 

SUBROUTINE FU N ( N V , T , V AR , DR V ) 



FCN SUBROUTINE IS USED FOR EVALUATING F 0 NCT 10 NS ( I N PUT) 



C 



C 



c 



IMPLICIT R E AL*8 (A-H. 0-2) 
COMMON F ' “ ^ 

*AKRKT (8, 

^FSM KH (8, 

COMMON/* 



8,0) ,FSj8,8f ,GQGT (0) ,AK (0,3) ,R (3, 3j , 
8) , DF (0. 8) , FT (8, 8) , PSMKHT (0,8) , DPT (8 , 



8 ) , 



COMMON/KTR/N. NS , N PD 

DIMENSION U (3b\ , V (8 ,8) ,P (35 ) . UD (36) . V 
OVA3 (13b) .DRV ( 1 J b ) ,C(2u[.WK(136,4),P6( 
DIMENSION 7MP1 (8,8) ,TMP2 (8, 8) ,TMP3 (8, 



VD (8 , 0) 
(3b) 

0 ) 



L=0 

DO 1 1=1, NS 
L = LO 

1 U (I) = VAR (L) 



DO 2 1=1, N 
DO 2 J=1,N 
L = L ♦ 1 

2 V (I , J)=VAR (L) 



3 



? 9 



DO 3 1=1, NS 
L = L ♦ 1 



P (I ) = VAP jfL) 
If ( i . Eg. C) KT 
KT= KT ♦ I 



IF (KT.GE.5) 
Vfcl IF (6,99) 

formaT('ot=' 



= N TD 



GO TO 15 
]o2b. 15) 



101 



c 



c 



c 



c 



c 



c 



c 



CALL OSWSM ru* , 1 ,0, N, 3) 

CALL 0SWFP1 ' V ' , 1 , V, 7, N,N ,3> 

CALL US W Sfl ( *P • , 1 , V , N, 3) 

CALL VMULFS (F . 0 . N, N , 8 , TM PI f 8) 

I F ( KT . LT . 5 ) CALL US WF M ( • ?U » , 3 ,T .IF 1 , 8 , 
CALL VMULS? (U,N.FT, N, 8,THP2 . 8) 

IF ( KT - LT - 5 ) CALL US WF H ( • UFT 1 , 3, TM?2, S 



• *,3) 
*,N, 3 ) 



DO 5 1=1 , N 
DO 4 J= 1 ,N 

TMP 1 (I , J) = TMP 1 (I, J) + TMP2 (l, J) 

TAP 1 (I t lj=TMP1 (1,1 ♦GQGX I) 

CALL VCVtFS (7MP1,N,7, UD) 

IF ( KT . LT - 5 T CALL U5WS M (• UDDT* .4 . U D JJ ^ 

CALL VKULFF(F f V,N,N,N,8 r 8,rrtPl,o.l!;R) " 

IF(KT.LT.S) CALL 0 S S F H ( 1 FV • 3 , T M P 1 # 8 . x v o . 

CALL VMULFF (V.FSMKHT, N,N,N, 6,8. TM?2,5 l ihx 
IF ( KT . LT. 5 ) CALL OS WF *1 (• ( PS-KH) T« ' 1 J ‘ =2U w M 

CALL VMULSF (U.N. D FT, N . 0. TMP 3 , 8) * - n VI , 8 , N , N , 

IF ( KT - LT • 5 ) CALL OS WF M ( 1 U 5 D FT • , 5 , T M P3 # N # N , 3) 

DO 7 1=1 ,N 
DO 6 o=1, N 

VD(I,J) =TMP1 (I, J) *TMP2(I,J) ♦TMP 3 (I,J) 

VD I.IUVD (I , if “GQGT ( I) 

IF(KT.Lt.5) CaLl USWFM(« 

CALL “ " " 

I F ( KT ^ , . 

CALL VMULSF (P.N.FSMKHT,N,8,TMP 



3 ) 



KT.LT.5) CALL US WF ft ( • VDD T • 4 , V D , 8 . k u 
LL VMULFS (PSMKH,P,N.N,S,TMP1 ,8) 

KT.LT.5j_ Call uswfm (• (F5-kh) £•, a, r i p * u 

L VMULSF (P f N,FSHKHT,N, 8, TMP2,8f N,N, 

I F£ KT . LT . 5 L CALL U S WF M ( • ? ( F S- KH) T > , 9 . - 



l all VMULF^(DF, V,N,N,N, 8 , 0,1 nrj,o.iEK\ 

IF ( KT . LT- 5 ) CALL U S W r M ( * 6 ? * v • ,4 , T 6 P 3 , ^ , N , N , 3 ) 

1=1 ,N 
J=1,N 

I ,J) = TMP 1 (I, J) +TMP2 (I, J) + TMP3 < 1 , .n 
V MUlFM (V f DF,N,N,N,8,8 f TMP3,8,is!>\ * 

-LT.5) CALL USWFMC VT> DP«, 5, fnpr c 



3 ) 



ifTMP 3 ) / 8 f t^r P2 ' 8 ' N ' N ' 3) 



DO 8 1=1 ,N 
DO 8 J=1 , N 
8 TMP 1 (I 
CALL ” 

I F ( KT 



,N,N,3) 



q 

10 



DO 10 1=1, N 
DO 9 J= 1 ,N 



TMP 3 (I, J)=TMP1 (I, J) ♦THP3 (I, J) ♦AKFKT (1 jx 
TMP 3 JI, I) = TMP 3 (I # lJ ♦GOGIfll 1 ' J) 

IF(KT.Lt.5) CALL U 5 W F M ( • fDjT* ,4,TMP3, 8 * m n 
CALL VCVTFS (T MP 3 , N . 8, PD) 

IFjKT.LT.5) CALL US«rV( , PDDT(SYM) • , 9 , r p N 1 3) 



DO 11 1=1, NS 
L = L ♦ 1 

1 1 DRV (L) = U D ( I ) 

DC 12 1 = 1, N 
DO 12 J=1,N 
L = L ♦ 1 

1 2 DRV (L) = V D ( I ,J) 

DO 13 1=1, NS 
L=L ♦ 1 

1 3 DRV <L) =P 0 ( I ) 

IF (KT.LT.5) CALL U SW F V ( • DR V • , 3 , DR V , M V 1 
PFTUEN ' 9 

END 
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